#include "pd_state_machine.h"
#include "typec_state_machine.h"
#include "hal_tcpc_operation.h"
#include "typec_pd_timer.h"
#include <string.h>

static void pd_pe_state_transition_to(uint8_t new_state);
void pd_dpm_policy_reset(void);

//#define PD_PE_STATE_MACHINE_DBG
#ifdef PD_PE_STATE_MACHINE_DBG
#define PD_PE_STATE_LOG(...)    printf(__VA_ARGS__)
static char *pd_pe_state_string[PD_PE_STATE_MAX] = {
    [PD_PE_INVALID]                   = "PE_INVALID",
    [PD_PE_ERROR_RECOVERY]            = "PE_ERROR_RECOVERY",
    [PD_PE_BIST_TEST_MODE]            = "PE_BIST_TEST_MODE",
    [PD_PE_BIST_CARRIER_MODE]         = "PE_BIST_CARRIER_MODE",

    [PD_PE_SRC_STARTUP]               = "PE_SRC_STARTUP",
    [PD_PE_SRC_DISCOVERY]             = "PE_SRC_DISCOVERY",
    [PD_PE_SRC_SEND_CAPABILITIES]     = "PE_SRC_SEND_CAPABILITIES",
    [PD_PE_SRC_NEGOTIATE_CAPABILITY]  = "PE_SRC_NEGOTIATE_CAPABILITY",
    [PD_PE_SRC_TRANSITION_SUPPLY1]    = "PE_SRC_TRANSITION_SUPPLY1",
    [PD_PE_SRC_TRANSITION_SUPPLY2]    = "PE_SRC_TRANSITION_SUPPLY2",
    [PD_PE_SRC_DISABLED]              = "PE_SRC_DISABLED",
    [PD_PE_SRC_CAPABILITY_RESPONSE]   = "PE_SRC_CAPABILITY_RESPONSE",
    [PD_PE_SRC_WAIT_NEW_CAPABILITIES] = "PE_SRC_WAIT_NEW_CAPABILITIES",
    [PD_PE_SRC_SEND_SOFT_RESET]       = "PE_SRC_SEND_SOFT_RESET",
    [PD_PE_SRC_SOFT_RESET]            = "PE_SRC_SOFT_RESET",
    [PD_PE_SRC_HARD_RESET_RECEIVED]   = "PE_SRC_HARD_RESET_RECEIVED",
    [PD_PE_SRC_TRANSITION_TO_DEFAULT] = "PE_SRC_TRANSITION_TO_DEFAULT",
    [PD_PE_SRC_HARD_RESET]            = "PE_SRC_HARD_RESET",
    [PD_PE_SRC_READY]                 = "PE_SRC_READY",

    [PD_PE_SNK_STARTUP]               = "PE_SNK_STARTUP",
    [PD_PE_SNK_DISCOVERY]             = "PE_SNK_DISCOVERY",
    [PD_PE_SNK_WAIT_FOR_CAPABILITIES] = "PE_SNK_WAIT_FOR_CAPABILITIES",
    [PD_PE_SNK_EVALUATE_CAPABILITY]   = "PE_SNK_EVALUATE_CAPABILITY",
    [PD_PE_SNK_SELECT_CAPABILITY]     = "PE_SNK_SELECT_CAPABILITY",
    [PD_PE_SNK_TRANSITION_SINK]       = "PE_SNK_TRANSITION_SINK",
    [PD_PE_SNK_DISABLED]              = "PE_SNK_DISABLED",
    [PD_PE_SNK_SEND_SOFT_RESET]       = "PE_SNK_SEND_SOFT_RESET",
    [PD_PE_SNK_SOFT_RESET]            = "PE_SNK_SOFT_RESET",
    [PD_PE_SNK_TRANSITION_TO_DEFAULT] = "PE_SNK_TRANSITION_TO_DEFAULT",
    [PD_PE_SNK_HARD_RESET]            = "PE_SNK_HARD_RESET",
    [PD_PE_SNK_READY]                 = "PE_SNK_READY",

    [PD_PE_GIVE_MANUFACTURER_INFO]    = "PE_GIVE_MANUFACTURER_INFO",
    [PD_PE_GIVE_SINK_CAP]             = "PE_GIVE_SINK_CAP",
    [PD_PE_GIVE_SOURCE_CAP]           = "PE_GIVE_SRC_CAP",
    [PD_PE_GIVE_SINK_CAP_EXT]         = "PE_GIVE_SINK_CAP_EXT",
    [PD_PE_GIVE_SOURCE_CAP_EXT]       = "PE_GIVE_SRC_CAP_EXT",
    [PD_PE_GIVE_REVISION]             = "PE_GIVE_REVISION",
    [PD_PE_GET_SINK_CAP]              = "PE_GET_SINK_CAP",
    [PD_PE_GET_SOURCE_CAP]            = "PE_GET_SOURCE_CAP",
    [PD_PE_GET_SINK_CAP]              = "PE_GET_SINK_CAP",
    [PD_PE_GET_SOURCE_CAP]            = "PE_GET_SOURCE_CAP",
    [PD_PE_CHUNK_RECEIVED]            = "PE_CHUNK_RECEIVED",
    [PD_PE_SEND_NOT_SUPPORTED]        = "PE_SEND_NOT_SUPPORTED",

    [PD_PE_PRS_SRC_SNK_EVALUATE_SWAP] = "PE_PRS_SRC_SNK_EVALUATE_SWAP",
    [PD_PE_PRS_SRC_SNK_REJECT_SWAP]   = "PE_PRS_SRC_SNK_REJECT_SWAP",
    [PD_PE_PRS_SRC_SNK_ACCEPT_SWAP]   = "PE_PRS_SRC_SNK_ACCEPT_SWAP",
    [PD_PE_PRS_SRC_SNK_SEND_SWAP]     = "PE_PRS_SRC_SNK_SEND_SWAP",
    [PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF] = "PE_PRS_SRC_SNK_TRANSITION_TO_OFF",
    [PD_PE_PRS_SRC_SNK_ASSERT_RD]     = "PE_PRS_SRC_SNK_ASSERT_RD",
    [PD_PE_PRS_SRC_SNK_WAIT_SOURCE_ON]    = "PE_PRS_SRC_SNK_WAIT_SOURCE_ON",
    [PD_PE_PRS_SNK_SRC_EVALUATE_SWAP] = "PE_PRS_SNK_SRC_EVALUATE_SWAP",
    [PD_PE_PRS_SNK_SRC_REJECT_SWAP]   = "PE_PRS_SNK_SRC_REJECT_SWAP",
    [PD_PE_PRS_SNK_SRC_ACCEPT_SWAP]   = "PE_PRS_SNK_SRC_ACCEPT_SWAP",
    [PD_PE_PRS_SNK_SRC_SEND_SWAP]     = "PE_PRS_SNK_SRC_SEND_SWAP",
    [PD_PE_PRS_SNK_SRC_TRANSITION_TO_OFF] = "PE_PRS_SNK_SRC_TRANSITION_TO_OFF",
    [PD_PE_PRS_SNK_SRC_ASSERT_RP]     = "PE_PRS_SNK_SRC_ASSERT_RP",
    [PD_PE_PRS_SNK_SRC_SOURCE_ON]     = "PE_PRS_SNK_SRC_SOURCE_ON",

    [PD_PE_DRS_DFP_UFP_EVALUATE_SWAP] = "PE_DRS_DFP_UFP_EVALUATE_SWAP",
    [PD_PE_DRS_DFP_UFP_REJECT_SWAP]   = "PE_DRS_DFP_UFP_REJECT_SWAP",
    [PD_PE_DRS_DFP_UFP_ACCEPT_SWAP]   = "PE_DRS_DFP_UFP_ACCEPT_SWAP",
    [PD_PE_DRS_DFP_UFP_SEND_SWAP]     = "PE_DRS_DFP_UFP_SEND_SWAP",
    [PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP] = "PE_DRS_DFP_UFP_CHANGE_TO_UFP",
    [PD_PE_DRS_UFP_DFP_EVALUATE_SWAP] = "PE_DRS_UFP_DFP_EVALUATE_SWAP",
    [PD_PE_DRS_UFP_DFP_REJECT_SWAP]   = "PE_DRS_UFP_DFP_REJECT_SWAP",
    [PD_PE_DRS_UFP_DFP_ACCEPT_SWAP]   = "PE_DRS_UFP_DFP_ACCEPT_SWAP",
    [PD_PE_DRS_UFP_DFP_SEND_SWAP]     = "PE_DRS_UFP_DFP_SEND_SWAP",
    [PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP] = "PE_DRS_UFP_DFP_CHANGE_TO_DFP",

    [PD_PE_VCS_EVALUATE_SWAP]         = "PE_VCS_EVALUATE_SWAP",
    [PD_PE_VCS_REJECT_VCONN_SWAP]     = "PE_VCS_REJECT_VCONN_SWAP",
    [PD_PE_VCS_ACCEPT_SWAP]           = "PE_VCS_ACCEPT_SWAP",
    [PD_PE_VCS_TURN_ON_VCONN]         = "PE_VCS_TURN_ON_VCONN",
    [PD_PE_VCS_SEND_PS_RDY]           = "PE_VCS_SEND_PS_RDY",
    [PD_PE_VCS_WAIT_FOR_VCONN]        = "PE_VCS_WAIT_FOR_VCONN",
    [PD_PE_VCS_TURN_OFF_VCONN]        = "PE_VCS_TURN_OFF_VCONN",

    [PD_PE_RESP_VDM_GET_IDENTITY]     = "PE_RESP_VDM_GET_IDENTITY",
    [PD_PE_RESP_VDM_GET_IDENTITY_NAK] = "PE_RESP_VDM_GET_IDENTITY_NAK",
    [PD_PE_RESP_VDM_SEND_IDENTITY]    = "PE_RESP_VDM_SEND_IDENTITY",
    [PD_PE_INIT_PORT_VDM_IDENTITY_REQUEST]  = "PE_INIT_PORT_VDM_IDENTITY_REQUEST",
    [PD_PE_INIT_PORT_VDM_IDENTITY_ACKED]    = "PE_INIT_PORT_VDM_IDENTITY_ACKED",
    [PD_PE_INIT_PORT_VDM_IDENTITY_NAKED]    = "PE_INIT_PORT_VDM_IDENTITY_NAKED",
    [PD_PE_RESP_VDM_GET_SVIDS_NAK]    = "PE_RESP_VDM_GET_SVIDS_NAK",
    [PD_PE_RESP_VDM_GET_MODES_NAK]    = "PE_RESP_VDM_GET_MODES_NAK",
    [PD_PE_RESP_VDM_ENTER_MODE_NAK]   = "PE_RESP_VDM_ENTER_MODE_NAK",
    [PD_PE_RESP_VDM_EXIT_MODE_NAK]    = "PE_RESP_VDM_EXIT_MODE_NAK",
};
#else
#define PD_PE_STATE_LOG(...)    ((void)0)
#endif

void pd_port_init(void)
{
    tpc_port.pd_port.power_role = PD_POWER_ROLE_SNK;
    tpc_port.pd_port.data_role = PD_DATA_ROLE_UFP;
    tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_NONE;
    tpc_port.pd_port.need_vconn = false;
    tpc_port.pd_port.pd_revision[0] = PD_REVISION_3x; // SOP
    tpc_port.pd_port.pd_revision[1] = PD_REVISION_3x; // SOPP
    tpc_port.pd_port.pd_vdm_revision[0] = VDM_VERSION_MINOR_21; // SOP
    tpc_port.pd_port.pd_vdm_revision[1] = VDM_VERSION_MINOR_21; // SOPP
    tpc_port.pd_port.vdm_svid = 0xFF00;
    tpc_port.pd_port.vdm_mode_idx = 0;
    tpc_port.pd_port.pd_connected = false;
    tpc_port.pd_port.pd_rev_synced = false;
    tpc_port.pd_port.request_mv = 0;
    tpc_port.pd_port.request_ma = 0;
    tpc_port.pd_port.rdo = 0;
    tpc_port.pd_port.remote_src_cap.number = 0;
    tpc_port.pd_port.remote_snk_cap.number = 0;
    tpc_port.pd_port.local_snk_cap.number = 3;
    tpc_port.pd_port.local_snk_cap.pdos[0] = PD_MSG_PDO_TYPE_FIXED | \
                                             PD_MSG_PDO_FIXED(5000, 3000) | \
                                             PD_MSG_PDO_FIXED_DRP | \
                                             PD_MSG_PDO_FIXED_DRD;
    tpc_port.pd_port.local_snk_cap.pdos[1] = PD_MSG_PDO_TYPE_FIXED | \
                                             PD_MSG_PDO_FIXED(9000, 3000);
    tpc_port.pd_port.local_snk_cap.pdos[2] = PD_MSG_PDO_TYPE_APDO | \
                                             PD_MSG_PDO_TYPE_APDO_SPR_PPS | \
                                             PD_MSG_PDO_APDO_PPS(5000, 11000, 3000);
    tpc_port.pd_port.local_src_cap.number = 1;
    tpc_port.pd_port.local_src_cap.pdos[0] = PD_MSG_PDO_TYPE_FIXED | \
                                             PD_MSG_PDO_FIXED(5000, 3000) | \
                                             PD_MSG_PDO_FIXED_DRP | \
                                             PD_MSG_PDO_FIXED_DRD;
    tpc_port.pd_port.vdos[0] = 0;  // SVDM Header
    tpc_port.pd_port.vdos[1] = PD_MSG_VDO_IDH(VDM_IDH_NO_USB_HOST, VDM_IDH_NO_USB_DEVICE, 
                                              VDM_IDH_UFPTYPE_PSD, VDM_IDH_MODAL_OP_NOT_SUPPORT, 
                                              VDM_IDH_DFPTYPE_USBHOST, VDM_IDH_CONNTYPE_RECEPTACLE, 
                                              VDM_IDH_USB_VENDOR_ID);
    tpc_port.pd_port.vdos[2] = PD_MSG_VDO_CSTAT(0);
    tpc_port.pd_port.vdos[3] = PD_MSG_VDO_PRODUCT(VDM_PRODUCT_PID, VDM_PRODUCT_BCDDEVICE);
    tpc_port.pd_port.vdos[4] = PD_MSG_VDO_UFP(0, 0, 0, 0, 0, 0);  // UFP VDO
    tpc_port.pd_port.vdos[5] = 0;  // Pad
    tpc_port.pd_port.vdos[6] = PD_MSG_VDO_DFP(0, 0);  // DFP VDO

    /* dpm */
    pd_dpm_policy_reset();
}

static bool pd_is_rev30(void)
{
    return tpc_port.pd_port.pd_revision[0] >= PD_REVISION_3x;
}

void pd_policy_engine_reset(void)
{
    tpc_port.pd_pe.pe_state_old = PD_PE_INVALID;
    tpc_port.pd_pe.pe_state = PD_PE_INVALID;
    tpc_port.pd_pe.explicit_contract = false;
    tpc_port.pd_pe.in_pr_swap = false;
    tpc_port.pd_pe.prs_to_src = false;
    tpc_port.pd_pe.in_hard_reset = false;
}

/* PD Protocol Layer */
void pd_protocol_layer_reset(void)
{
    tpc_port.pd_protocol.ncapscount = 0;
    tpc_port.pd_protocol.nmessageidcount = 0;
    tpc_port.pd_protocol.nhardresetcount = 0;
}

void pd_protocol_handle_soft_reset(void)
{
    tpc_port.pd_protocol.nmessageidcount = 0;
    // clear rx msg fifo
    msg_rx_fifo_init();
    tpc_port.pd_msg_received = false;
}

void delay_1us(uint32_t us);
uint8_t pd_protocol_send_message(uint8_t frame_type, bool ext, uint8_t cnt, uint8_t msg_type, const uint32_t *data)
{
    int ret = PD_PROTO_TX_FAILED;
    uint16_t msg_hdr;
    uint8_t msg_id, pd_revision, retry_cnt;
    uint8_t tx_result = 0;

    msg_id = tpc_port.pd_protocol.nmessageidcount;
    pd_revision = tpc_port.pd_port.pd_revision[0];
    if (pd_revision >= PD_REVISION_3x) {
        retry_cnt = 2;
    } else {
        retry_cnt = 3;
    }
    msg_hdr = PD_MSG_HEADER_SOP(ext, cnt, msg_id, tpc_port.pd_port.power_role, \
                                pd_revision, tpc_port.pd_port.data_role, msg_type);

    ret = tcpc_transmit(frame_type, msg_hdr, retry_cnt, data);
    if (ret)
        return ret;

    /* wait for tx result: success, fail or discard */
/*
    do {
        delay_1us(200);
        ret = tcpc_get_tx_result(&tx_result);
        if (ret)
            return ret;
    } while(!tx_result);
*/

    while (1) {
        if (typec_check_alert()) {
            ret = tcpc_get_tx_result(&tx_result);
            if (ret)
                return ret;
            if (tx_result)
                break;
        }
    }

    if (tx_result & TCPC_ALERT_TX_SUCCESS) {
        tpc_port.pd_protocol.nmessageidcount = (msg_id + 1) % 8;
        ret = PD_PROTO_TX_SUCCESS;
    } else if (tx_result & TCPC_ALERT_TX_FAILED){
        ret = PD_PROTO_TX_FAILED;
    } else if ((tx_result & TCPC_ALERT_TX_DISCARDED) || (tx_result & TCPC_ALERT_RX_STATUS)) {
        ret = PD_PROTO_TX_DISCARDED;
    }
    /**
     * Hard Reset sent will trigger tx_success and tx_failed at the same time
     */
    tcpc_alert_status_clear(tx_result & ~TCPC_ALERT_RX_STATUS);

    return ret;
}

void pd_protocol_process_tx(uint8_t frame_type, bool ext, uint8_t cnt, uint8_t msg_type, const uint32_t *data)
{
    uint8_t tx_result;

    if (!pd_is_rev30()) {
        if ((cnt == 0) && (msg_type == MSG_CTRL_NOT_SUPPORTED)) {
            // PD 2.0 use REJECT, instead of NOT_SUPPORTED
            msg_type = MSG_CTRL_REJECT;
        }
    }

    tx_result = pd_protocol_send_message(frame_type, ext, cnt, msg_type, data);

    if (frame_type == FRAME_HARD_RESET)
        return;

    if (tx_result == PD_PROTO_TX_SUCCESS) {
        tpc_port.pd_event = PD_EVENT_PROTO_TX_SUCC;
        tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_MESSAGE_SENT;
    } else if (tx_result == PD_PROTO_TX_FAILED) {
        tpc_port.pd_event = PD_EVENT_PROTO_TX_FAILED;
        tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_TRANSMISSION_ERROR;
    } else if (tx_result == PD_PROTO_TX_DISCARDED) {
        tpc_port.pd_event = PD_EVENT_PROTO_TX_DISCARD;
        tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_TRANSMISSION_ERROR;
    }

    return;
}

static void pd_protocol_set_sink_tx(uint8_t cc)
{
    if (!pd_is_rev30()) {
        return;
    }

    if ((cc != PD_PROTO_SINK_TX_OK) && (cc != PD_PROTO_SINK_TX_NG)) {
        return;
    }


    if (tpc_port.rp_level == cc) {
        return;
    }
    tpc_port.rp_level = cc;
    PD_PE_STATE_LOG("src set sink_tx_%s\n", (cc == PD_PROTO_SINK_TX_OK) ? "ok" : "ng");
    tcpc_set_local_rp_level(cc);
}

bool pd_protocol_check_sinktxok(void)
{
    bool sinktxok = false;

    if (!tpc_port.pd_pe.explicit_contract) {
        return false;
    }

    tcpc_get_cc_status(&tpc_port.cc1_sts, &tpc_port.cc2_sts);

    if (pd_is_rev30()) {
        if (tpc_port.cc_orient == TYPEC_CC1_COMM) {
            if (tpc_port.cc1_sts == TYPEC_CC_SNK_POWER_3A) {
                sinktxok = true;
            }
        } else {
            if (tpc_port.cc2_sts == TYPEC_CC_SNK_POWER_3A) {
                sinktxok = true;
            }
        }
    } else {
        sinktxok = true;
    }

    return sinktxok;
}

/* PD Device Policy Manager */
void pd_dpm_update_dpm_flags(uint32_t pdo)
{
    if ((pdo & PD_MSG_PDO_TYPE_MASK) == PD_MSG_PDO_TYPE_FIXED) {
        // TODO
        // if (pdo & PD_MSG_PDO_FIXED_DRP)
    }
}

void pd_dpm_get_request_info(struct dpm_rdo_info *rdo_info)
{
    bool find_cap = false;
    int i, j;
    struct pd_port_power_capabilities snk_cap, src_cap;
    uint16_t snk_mv, src_mv, snk_ma, src_ma;
    uint16_t snk_max_v, snk_min_v, snk_max_c, src_max_v, src_min_v, src_max_c;

    snk_cap = tpc_port.pd_port.local_snk_cap;
    src_cap = tpc_port.pd_port.remote_src_cap;
    for (i = 0; i < snk_cap.number; i++) {
        for (j = 0; j < src_cap.number; j++) {
            if (PD_MSG_PDO_TYPE(snk_cap.pdos[i]) == PD_MSG_PDO_TYPE(src_cap.pdos[j])) {
                if (PD_MSG_PDO_TYPE_RAW(src_cap.pdos[j]) == PD_MSG_PDO_TYPE_FIXED_RAW) {
                    src_mv = PD_MSG_PDO_FIXED_GET_VOL(src_cap.pdos[j]);
                    src_ma = PD_MSG_PDO_FIXED_GET_CURR(src_cap.pdos[j]);
                    snk_mv = PD_MSG_PDO_FIXED_GET_VOL(snk_cap.pdos[i]);
                    snk_ma = PD_MSG_PDO_FIXED_GET_CURR(snk_cap.pdos[i]);
                    if (src_mv == snk_mv) {
                        if (src_ma >= snk_ma) {
                            find_cap = true;
                            rdo_info->pos = j + 1;
                            rdo_info->type = PD_MSG_PDO_TYPE_FIXED_RAW;
                            rdo_info->out_vol = snk_mv;
                            rdo_info->op_curr = snk_ma;
                        } else if (src_ma == 0) {
                            // suspend
                            find_cap = true;
                            rdo_info->pos = j + 1;
                            rdo_info->type = PD_MSG_PDO_TYPE_FIXED_RAW;
                            rdo_info->out_vol = snk_mv;
                            rdo_info->op_curr = 0;
                        }
                    }
                } else if ((PD_MSG_PDO_TYPE_RAW(src_cap.pdos[j]) == PD_MSG_PDO_TYPE_APDO_RAW) &&\
                           (PD_MSG_PDO_SUBTYPE_RAW(src_cap.pdos[j]) == PD_MSG_PDO_TYPE_APDO_SPR_PPS_RAW)) {
                    snk_max_v = PD_MSG_PDO_PPS_GET_MAX_VOL(snk_cap.pdos[i]);
                    snk_min_v = PD_MSG_PDO_PPS_GET_MIN_VOL(snk_cap.pdos[i]);
                    snk_max_c = PD_MSG_PDO_PPS_GET_MAX_CURR(snk_cap.pdos[i]);
                    src_max_v = PD_MSG_PDO_PPS_GET_MAX_VOL(src_cap.pdos[j]);
                    src_min_v = PD_MSG_PDO_PPS_GET_MIN_VOL(src_cap.pdos[j]);
                    src_max_c = PD_MSG_PDO_PPS_GET_MAX_CURR(src_cap.pdos[j]);
                    if ((snk_min_v >= src_min_v) && (snk_max_v <= src_max_v) && (snk_max_c <= src_max_c)) {
                        find_cap = true;
                        rdo_info->pos = j + 1;
                        rdo_info->type = PD_MSG_PDO_TYPE_APDO_RAW + PD_MSG_PDO_TYPE_APDO_SPR_PPS_RAW;
                        rdo_info->out_vol = snk_min_v;
                        rdo_info->op_curr = snk_max_c - 500;
                    }
                }
            }
        }
    }

    if (!find_cap) {
        rdo_info->pos = 1;
        rdo_info->type = PD_MSG_PDO_TYPE_FIXED_RAW;
        rdo_info->mismatch = true;
        rdo_info->out_vol = 5000;
        rdo_info->op_curr = 100;
    }

    tpc_port.pd_port.request_mv = rdo_info->out_vol;
    tpc_port.pd_port.request_ma = rdo_info->op_curr;
}

void pd_dpm_build_request(struct dpm_rdo_info rdo_info)
{
    tpc_port.pd_port.pps_rdo = false;

    PD_PE_STATE_LOG("local rdo pos %d, type %d, vol %d, curr %d\n", \
                     rdo_info.pos, rdo_info.type, rdo_info.out_vol, rdo_info.op_curr);

    if (rdo_info.type == PD_MSG_PDO_TYPE_FIXED_RAW) {
        tpc_port.pd_port.rdo = PD_MSG_RDO_FIXED(rdo_info.pos, rdo_info.op_curr);
    } else if ((rdo_info.type == (PD_MSG_PDO_TYPE_APDO_RAW + PD_MSG_PDO_TYPE_APDO_SPR_PPS_RAW))) {
        tpc_port.pd_port.rdo = PD_MSG_RDO_SPR_PPS(rdo_info.pos, rdo_info.out_vol, rdo_info.op_curr);
    } else {
        PD_PE_STATE_LOG("unsupported src pdo type!\n");
    }

    if (rdo_info.mismatch) {
        tpc_port.pd_port.rdo |= PD_MSG_RDO_CAP_MISMATCH;
    }
}

void pd_dpm_evaluate_src_caps(void) {
    uint8_t i, src_pdo_nr;
    struct dpm_rdo_info rdo_info;

    // extract remote src caps
    src_pdo_nr = PD_MSG_HEADER_CNT(tpc_port.pd_msg.msg_hdr);
    for (i = 0; i < src_pdo_nr; i++) {
        tpc_port.pd_port.remote_src_cap.pdos[i] = tpc_port.pd_msg.payload[i];
//        PD_PE_STATE_LOG("remote src cap pdo[%d]0x%08x\n", i, tpc_port.pd_port.remote_src_cap.pdos[i]);
    }
    tpc_port.pd_port.remote_src_cap.number = src_pdo_nr;
    pd_dpm_update_dpm_flags(tpc_port.pd_port.remote_src_cap.pdos[0]);

    // analyze local snk caps and remote src caps
    memset(&rdo_info, 0, sizeof(struct dpm_rdo_info));
    pd_dpm_get_request_info(&rdo_info);

    // build rdo
    pd_dpm_build_request(rdo_info);
}

void pd_dpm_sync_pd_revision(void)
{
    uint8_t rev = PD_MSG_HEADER_REV(tpc_port.pd_msg.msg_hdr);

    if (!tpc_port.pd_port.pd_rev_synced) {
        tpc_port.pd_port.pd_revision[0] = MIN(tpc_port.pd_port.pd_revision[0], rev);
        tpc_port.pd_port.pd_rev_synced = true;
//        PD_PE_STATE_LOG("pd revision=%d\n", tpc_port.pd_port.pd_revision[0]);
    }
}

void pd_dpm_sync_svdm_revision(void)
{
    uint8_t rev = PD_MSG_VDO_VER_MINOR(tpc_port.pd_msg.payload[0]);

    tpc_port.pd_port.pd_vdm_revision[0] = MIN(tpc_port.pd_port.pd_vdm_revision[0], rev);
//  PD_PE_STATE_LOG("pd vdm revision=%d\n", tpc_port.pd_port.pd_vdm_revision[0]);
}

bool pd_dpm_evaluate_request(void)
{
    bool accept = false;
    uint32_t rdo, pdo;
    uint8_t rdo_pos, mismatch;
    uint16_t pdo_cur, pdo_vol, rdo_cur;

    rdo = tpc_port.pd_msg.payload[0];
    rdo_pos = PD_MSG_RDO_OBJ_POS_RAW(rdo);

    if ((rdo_pos == 0) || (rdo_pos > tpc_port.pd_port.local_src_cap.number)) {
        return false;
    }

    pdo = tpc_port.pd_port.local_src_cap.pdos[rdo_pos - 1];
    switch (PD_MSG_PDO_TYPE_RAW(pdo)) {
    case PD_MSG_PDO_TYPE_FIXED_RAW:
        pdo_cur = PD_MSG_PDO_FIXED_GET_CURR(pdo);
        pdo_vol = PD_MSG_PDO_FIXED_GET_VOL(pdo);
        rdo_cur = PD_MSG_RDO_FIXED_GET_CURR(rdo);
        mismatch = PD_MSG_RDO_CAP_MISMATCH_RAW(rdo);
//        PD_PE_STATE_LOG("remote rdo cur %dmA mismatch %d, local pdo %dmV/%dmA\n", \
//                        rdo_cur, mismatch, pdo_vol, pdo_cur);
        if (rdo_cur <= pdo_cur) {
            accept = true;
            tpc_port.pd_port.request_mv = pdo_vol;
            tpc_port.pd_port.request_ma = rdo_cur;
        } else if (mismatch) {
            accept = true;
            tpc_port.pd_port.request_mv = pdo_vol;
            tpc_port.pd_port.request_ma = pdo_cur;
        }
        break;
    case PD_MSG_PDO_TYPE_BATT_RAW:
    case PD_MSG_PDO_TYPE_VAR_RAW:
    case PD_MSG_PDO_TYPE_APDO_RAW:
        // TODO: only support fixed pdo so far
        accept = false;
        break;
    }

    return accept;
}

void pd_dpm_policy_process(void)
{
    if (tpc_port.dpm_policy & PD_DPM_POLICY_PFEFER_SNK) {
        if (tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC) {
            pd_dpm_reactions_set(PD_DPM_REACTIONS_REQUEST_PRS);
        }
    }

    if (tpc_port.dpm_policy & PD_DPM_POLICY_PFEFER_DFP) {
        if (tpc_port.pd_port.data_role == PD_DATA_ROLE_UFP) {
            pd_dpm_reactions_set(PD_DPM_REACTIONS_REQUEST_DRS);
        }
    }

    if (!tpc_port.pd_port.remote_src_cap.number) {
        pd_dpm_reactions_set(PD_DPM_REACTIONS_GET_SRC_CAP);
    }

    if (!tpc_port.pd_port.remote_snk_cap.number) {
        pd_dpm_reactions_set(PD_DPM_REACTIONS_GET_SNK_CAP);
    }
}

void pd_dpm_policy_start(void)
{
    if (!tpc_port.dpm_ongoing) {
        pd_dpm_policy_process();
        tpc_port.dpm_ongoing = true;
    } else {
        // do nothing
    }
}

void pd_dpm_policy_reset(void)
{
    tpc_port.dpm_ongoing = false;
    tpc_port.dpm_tx_ready = false;
    tpc_port.dpm_policy = PD_DPM_POLICY_PFEFER_DFP | PD_DPM_POLICY_PFEFER_SNK;
    tpc_port.dpm_reactions = 0;
}

void pd_dpm_reactions_process(void)
{
    /**
     * Priority:
     * 1. Discover Identity
     * 2. Get Source Cap
     * 3. Get Sink Cap
     * 4. Request DR_Swap
     * 5. Reqeust PR_Swap
     * 6. Request new PDO if required
     * 7. TODO
     */
    if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_DISCOVER_ID) {
        pd_dpm_reactions_clear(PD_DPM_REACTIONS_DISCOVER_ID);
        pd_pe_state_transition_to(PD_PE_INIT_PORT_VDM_IDENTITY_REQUEST);
    } else if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_GET_SRC_CAP) {
        pd_dpm_reactions_clear(PD_DPM_REACTIONS_GET_SRC_CAP);
        pd_pe_state_transition_to(PD_PE_GET_SOURCE_CAP);
    } else if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_GET_SNK_CAP) {
        pd_dpm_reactions_clear(PD_DPM_REACTIONS_GET_SNK_CAP);
        pd_pe_state_transition_to(PD_PE_GET_SINK_CAP);
    } else if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_REQUEST_DRS) {
        if (tpc_port.pd_port.data_role == PD_DATA_ROLE_UFP) {
            pd_dpm_reactions_clear(PD_DPM_REACTIONS_REQUEST_DRS);
            pd_pe_state_transition_to(PD_PE_DRS_UFP_DFP_SEND_SWAP);
        }
    } else if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_REQUEST_PRS) {
        if ((tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC) && \
            (tpc_port.pd_port.remote_src_cap.number)) {
            pd_dpm_reactions_clear(PD_DPM_REACTIONS_REQUEST_PRS);
            if (tpc_port.pd_port.remote_src_cap.pdos[0] & PD_MSG_PDO_FIXED_UNCONSTRAINED) {
                pd_pe_state_transition_to(PD_PE_PRS_SRC_SNK_SEND_SWAP);
            }
        }
    } else if (tpc_port.dpm_reactions & PD_DPM_REACTIONS_POWER_REQUEST) {
        if (tpc_port.pd_port.power_role == PD_POWER_ROLE_SNK) {
            // build new tpc_port.pd_port.rdo
            pd_dpm_reactions_clear(PD_DPM_REACTIONS_POWER_REQUEST);
            pd_pe_state_transition_to(PD_PE_SNK_SELECT_CAPABILITY);
        }
    }
}

void pd_local_hw_reset(void)
{
    // clear tx flag and rx buffer
    tcpc_alert_status_clear(TCPC_ALERT_RX_HARD_RST | TCPC_ALERT_RX_OVERFLOW | TCPC_ALERT_RX_STATUS | \
                            TCPC_ALERT_TX_SUCCESS | TCPC_ALERT_TX_DISCARDED | TCPC_ALERT_TX_FAILED);

    // restore local rp level
    tcpc_set_local_rp_level(tpc_port.local_rplvl);
    tpc_port.rp_level = tpc_port.local_rplvl;
}

void pd_port_reset(void)
{
    /* pd event, timer, pd_msg */
    tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
    tpc_port.pd_event = PD_EVENT_NONE;
    tpc_port.pd_msg_received = false;
    memset(&tpc_port.pd_msg, 0, sizeof(struct pd_message));
    msg_rx_fifo_init();
    pd_timer_disable_all();

    /* pd protocol, policy engine, dpm */
    pd_port_init();
    pd_protocol_layer_reset();
    pd_policy_engine_reset();

    /* power supply */
    tpc_port.vbus_on = false;
    tpc_port.out_mv = 0;
    tpc_port.out_ma = 0;
    tpc_port.in_ma = 0;

    /* hardware(tcpc) */
    pd_local_hw_reset();
    tcpc_set_rx_enable(0);
}

static void pd_pe_src_snk_state_exit(uint8_t old_state)
{
    // TODO
    switch (old_state) {
    case PD_PE_BIST_TEST_MODE:
        tcpc_set_bist_test_mode(false);
        break;
    case PD_PE_BIST_CARRIER_MODE:
        pd_timer_disable(PD_TIMER_BISTCONTMODE);
        break;
    default:
        break;
    }
}

static void pd_pe_src_state_entry(uint8_t new_state)
{
    switch (new_state) {
    case PD_PE_SRC_STARTUP:
        pd_protocol_layer_reset();
        tpc_port.pd_port.power_role = PD_POWER_ROLE_SRC;
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        tcpc_set_rx_enable(TCPC_RX_DETECT_HARD_RESET);
        if (tpc_port.pd_pe.prs_to_src) {
            tpc_port.pd_pe.prs_to_src = false;
            pd_timer_enable(PD_TIMER_SWAPSOURCESTART, PD_TIMER_T_SWAPSOURCESTART, false);
        } else {
            tpc_port.pd_event = PD_EVENT_PROTO_RST_DONE;
        }
        break;
    case PD_PE_SRC_DISCOVERY:
        pd_timer_enable(PD_TIMER_SOURCECAPABILITY, PD_TIMER_T_SOURCECAPABILITY, false);
        break;
    case PD_PE_SRC_SEND_CAPABILITIES:
        tcpc_set_rx_enable(TCPC_RX_DETECT_SOP | TCPC_RX_DETECT_HARD_RESET);
        pd_protocol_process_tx(FRAME_SOP, false, tpc_port.pd_port.local_src_cap.number, 
                               MSG_DATA_SOURCE_CAPABILITIES, tpc_port.pd_port.local_src_cap.pdos);
        tpc_port.pd_protocol.ncapscount++;
        break;
    case PD_PE_SRC_NEGOTIATE_CAPABILITY:
        pd_dpm_sync_pd_revision();
        if (pd_dpm_evaluate_request()) {
            tpc_port.pd_event = PD_EVENT_DPM_ACCEPT;
        } else {
            tpc_port.pd_event = PD_EVENT_DPM_REJECT;
        }
        break;
    case PD_PE_SRC_TRANSITION_SUPPLY1:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_SRC_TRANSITION_SUPPLY2:
        typec_vbus_control(true, tpc_port.pd_port.request_mv, tpc_port.pd_port.request_ma);
        pd_timer_enable(PD_TIMER_SRCTRANSREQ, PD_TIMER_T_SRCTRANSREQ, false);
        break;
    case PD_PE_SRC_CAPABILITY_RESPONSE:
        // TODO: maybe send WAIT
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_REJECT, NULL);
        break;
    case PD_PE_SRC_WAIT_NEW_CAPABILITIES:
        // TODO: change source capabilites
        tpc_port.pd_event = PD_EVENT_DPM_READY;
    case PD_PE_SRC_HARD_RESET:
        tcpc_set_rx_enable(0);
        pd_protocol_process_tx(FRAME_HARD_RESET, false, 0, 0, NULL);
        pd_timer_enable(PD_TIMER_NORESPONSE, PD_TIMER_T_NORESPONSE, true);
        tpc_port.pd_protocol.nhardresetcount++;
        pd_timer_enable(PD_TIMER_PSHARDRESET, PD_TIMER_T_PSHARDRESET, false);
        break;
    case PD_PE_SRC_TRANSITION_TO_DEFAULT:
        pd_protocol_handle_soft_reset();
        tpc_port.pd_pe.explicit_contract = false;
        tpc_port.pd_port.pd_rev_synced = false;
        tpc_port.pd_port.pd_revision[0] = PD_REVISION_3x;
        tpc_port.pd_port.pd_revision[1] = PD_REVISION_3x;
        pd_local_hw_reset();
        tpc_port.pd_port.data_role = PD_DATA_ROLE_DFP;
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        if ((tpc_port.pd_port.vconn_role == PD_VCONN_ROLE_SRC)) {
            tcpc_set_vconn(false);
        }
        pd_timer_enable(PD_TIMER_SRCRECOVER, PD_TIMER_T_SRCRECOVER, false);
        break;
    case PD_PE_SRC_HARD_RESET_RECEIVED:
        tcpc_set_rx_enable(0);
        pd_timer_enable(PD_TIMER_NORESPONSE, PD_TIMER_T_NORESPONSE, true);
        pd_timer_enable(PD_TIMER_PSHARDRESET, PD_TIMER_T_PSHARDRESET, false);
        break;
    case PD_PE_SRC_SEND_SOFT_RESET:
        // Hard Reset shall be triggered within tHardReset(5ms) if Soft_Reset failed to send
        pd_protocol_handle_soft_reset();
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_SOFT_RESET, NULL);
        break;
    case PD_PE_SRC_SOFT_RESET:
        pd_protocol_handle_soft_reset();
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_SRC_DISABLED:
        tcpc_set_rx_enable(TCPC_RX_DETECT_HARD_RESET);
        break;
    case PD_PE_SRC_READY:
        /** 
         * TODO: 
         * 1. Run DiscoverIdentityTimer for Cable Discovery
         * 2. Run SourcePPSCommTimer if PPS APDO accepted
         * 3. Run SourceEPRKeepAliveTimer if EPR Mode entered
         * 4. Start new AMS for DPM Request
         * 5. Handle received message
         */
        if (!tpc_port.pd_port.pd_connected) {
            // first pd connected
            pd_dpm_reactions_set(PD_DPM_REACTIONS_DISCOVER_ID);
        }
        if (!tpc_port.dpm_ongoing) {
            pd_timer_disable(PD_TIMER_DELAY_START_DPM);
            pd_timer_enable(PD_TIMER_DELAY_START_DPM, PD_TIMER_T_DELAY_START_DPM, true);
        }
        tpc_port.pd_port.pd_connected = true;
        tpc_port.pd_pe.explicit_contract = true;
        break;
    default:
        break;
    }
}

static void pd_pe_snk_state_entry(uint8_t new_state)
{
    uint32_t sel_src_pdo;

    switch (new_state) {
    case PD_PE_SNK_STARTUP:
        pd_protocol_layer_reset();
        tpc_port.pd_port.power_role = PD_POWER_ROLE_SNK;
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        tcpc_set_rx_enable(TCPC_RX_DETECT_HARD_RESET);
        tpc_port.pd_event = PD_EVENT_PROTO_RST_DONE;
        break;
    case PD_PE_SNK_DISCOVERY:
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
            tpc_port.pd_event = PD_EVENT_VBUS_VSAFE5V;
        }
        break;
    case PD_PE_SNK_WAIT_FOR_CAPABILITIES:
        if (tpc_port.pd_pe.in_hard_reset) {
            tpc_port.pd_pe.in_hard_reset = false;
        }
        tcpc_set_rx_enable(TCPC_RX_DETECT_SOP | TCPC_RX_DETECT_HARD_RESET);
        pd_timer_enable(PD_TIMER_SINKWAITCAP, PD_TIMER_T_SINKWAITCAP, false);
        break;
    case PD_PE_SNK_EVALUATE_CAPABILITY:
        pd_dpm_sync_pd_revision();
        pd_dpm_evaluate_src_caps();
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    case PD_PE_SNK_SELECT_CAPABILITY:
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_REQUEST, &tpc_port.pd_port.rdo);
        break;
    case PD_PE_SNK_TRANSITION_SINK:
        sel_src_pdo = tpc_port.pd_port.remote_src_cap.pdos[PD_MSG_RDO_OBJ_POS_RAW(tpc_port.pd_port.rdo) - 1];
        if ((PD_MSG_PDO_TYPE_RAW(sel_src_pdo) == PD_MSG_PDO_TYPE_APDO_RAW) && \
            (PD_MSG_PDO_SUBTYPE_RAW(sel_src_pdo) == PD_MSG_PDO_TYPE_APDO_SPR_PPS_RAW)) {
            tpc_port.pd_port.pps_rdo = true;
        } else {
            tpc_port.pd_port.pps_rdo = false;
        }
        pd_timer_enable(PD_TIMER_PSTRANSITION, PD_TIMER_T_PSTRANSITION, false);
        break;
    case PD_PE_SNK_TRANSITION_TO_DEFAULT:
        pd_protocol_handle_soft_reset();
        tpc_port.pd_pe.explicit_contract = false;
        tpc_port.pd_port.pd_rev_synced = false;
        tpc_port.pd_port.pd_revision[0] = PD_REVISION_3x;
        tpc_port.pd_port.pd_revision[1] = PD_REVISION_3x;
        tcpc_set_rx_enable(0);
        pd_local_hw_reset();
        tpc_port.pd_port.data_role = PD_DATA_ROLE_UFP;
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        if (tpc_port.pd_port.vconn_role == PD_VCONN_ROLE_SRC) {
            tcpc_set_vconn(false);
        }
        typec_ibus_control(100);
        if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
            tpc_port.pd_event = PD_EVENT_VBUS_VSAFE0V;
        }
        break;
    case PD_PE_SNK_HARD_RESET:
        tcpc_set_rx_enable(0);
        if (tpc_port.pd_protocol.nhardresetcount > 2) {
            // no response from remote
            tpc_port.pd_pe.pe_state = PD_PE_SNK_DISABLED;
            PD_PE_STATE_LOG("[%d]**PD_PE_SNK_DISABLED**\n", g_sys_ticks);
        } else {
            pd_protocol_process_tx(FRAME_HARD_RESET, false, 0, 0, NULL);
            tpc_port.pd_protocol.nhardresetcount++;
            tpc_port.pd_event = PD_EVENT_HARD_RST_COMPLETE;
        }
        break;
    case PD_PE_SNK_SEND_SOFT_RESET:
        pd_protocol_handle_soft_reset();
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_SOFT_RESET, NULL);
        break;
    case PD_PE_SNK_SOFT_RESET:
        pd_protocol_handle_soft_reset();
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_SNK_READY:
        /** 
         * TODO: 
         * 1. SinkRequestTimer for Wait
         * 2. Cable Discover Identify
         * 3. SinkPPSPeriodicTimer for PPS Sink
         * 4. Get_Sink_Cap if support FRS
         * 5. Start new AMS for DPM Request
         */
        typec_ibus_control(tpc_port.pd_port.request_ma);
        if (tpc_port.pd_port.pps_rdo) {
            pd_timer_enable(PD_TIMER_SINKPPSPERIODIC, PD_TIMER_T_SINKPPSPERIODIC, true);
        } else {
            pd_timer_disable(PD_TIMER_SINKPPSPERIODIC);
        }
        if (!tpc_port.pd_port.pd_connected) {
            // first pd connected
//            pd_dpm_reactions_set(PD_DPM_REACTIONS_DISCOVER_ID);
        }
        if (!tpc_port.dpm_ongoing) {
            pd_timer_disable(PD_TIMER_DELAY_START_DPM);
            pd_timer_enable(PD_TIMER_DELAY_START_DPM, PD_TIMER_T_DELAY_START_DPM, true);
        }
        tpc_port.pd_port.pd_connected = true;
        tpc_port.pd_pe.explicit_contract = true;
        break;
    default:
        break;
    }
}

static void pd_pe_give_manufacturer_info_entry(void)
{
    struct pd_msg_manufacturer_info manu_info;
    const char manufacturer_string[8] = "sgmicro";
    const char not_supported_string[14] = "Not Supported";
    uint8_t *ext_data = (uint8_t *)(tpc_port.pd_msg.payload) + 2;
    uint8_t manu_info_tgt, manu_info_ref;
    uint8_t data_block_cnt;

    memset(&manu_info, 0, sizeof(struct pd_msg_manufacturer_info));
    manu_info_tgt = *ext_data;
    manu_info_ref = *(ext_data + 1);
    if ((manu_info_tgt > 1) || (manu_info_ref > 7)) {
        manu_info.ext_header = PD_MSG_EXT_HEADER(1, 0, 0, 18);
        manu_info.vid = 0xFFFF;
        manu_info.pid = 0x0;
        strncpy(manu_info.manu_str, not_supported_string, 14);
        data_block_cnt = 5;
    } else {
        manu_info.ext_header = PD_MSG_EXT_HEADER(1, 0, 0, 12);
        manu_info.vid = VDM_IDH_USB_VENDOR_ID;
        manu_info.pid = VDM_PRODUCT_PID;
        strncpy(manu_info.manu_str, manufacturer_string, 8);
        data_block_cnt = 4;
    }
    pd_protocol_process_tx(FRAME_SOP, true, data_block_cnt, MSG_EXT_MANUFACTURER_INFO, (uint32_t *)&manu_info);
}

static void pd_pe_give_source_cap_ext_entry(void)
{
    struct pd_msg_src_caps_ext src_caps_ext;
    uint8_t data_block_cnt;

    memset(&src_caps_ext, 0, sizeof(struct pd_msg_src_caps_ext));
    src_caps_ext.ext_header = PD_MSG_EXT_HEADER(1, 0, 0, 25);
    src_caps_ext.vid = VDM_IDH_USB_VENDOR_ID;
    src_caps_ext.pid = VDM_PRODUCT_PID;
    src_caps_ext.fw_ver = 1;
    src_caps_ext.hw_ver = 1;
    src_caps_ext.slot_no = 0;
    src_caps_ext.spr_pdp = 15;
    data_block_cnt = 7;

    pd_protocol_process_tx(FRAME_SOP, true, data_block_cnt, MSG_EXT_SOURCE_CAPABILITIES_EXTENDED, 
                           (uint32_t *)&src_caps_ext);
}

static void pd_pe_give_sink_cap_ext_entry(void)
{
    struct pd_msg_snk_caps_ext snk_caps_ext;
    uint8_t data_block_cnt;

    memset(&snk_caps_ext, 0, sizeof(struct pd_msg_snk_caps_ext));
    snk_caps_ext.ext_header = PD_MSG_EXT_HEADER(1, 0, 0, 24);
    snk_caps_ext.vid = VDM_IDH_USB_VENDOR_ID;
    snk_caps_ext.pid = VDM_PRODUCT_PID;
    snk_caps_ext.fw_ver = 1;
    snk_caps_ext.hw_ver = 1;
    snk_caps_ext.spr_op_pdp = 27;
    snk_caps_ext.spr_max_pdp = 27;
    data_block_cnt = 7;

    pd_protocol_process_tx(FRAME_SOP, true, data_block_cnt, MSG_EXT_SINK_CAPABILITIES_EXTENDED, 
                           (uint32_t *)&snk_caps_ext);
}

static void pd_pe_message_state_entry(uint8_t new_state)
{
//    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;
    uint32_t data_object;

    switch (new_state) {
    case PD_PE_GIVE_MANUFACTURER_INFO:
        pd_pe_give_manufacturer_info_entry();
        break;
    case PD_PE_GIVE_SINK_CAP:
        if (pd_is_rev30()) {
            pd_protocol_process_tx(FRAME_SOP, false, tpc_port.pd_port.local_snk_cap.number, 
                MSG_DATA_SINK_CAPABILITIES, tpc_port.pd_port.local_snk_cap.pdos);
        } else {
            pd_protocol_process_tx(FRAME_SOP, false, tpc_port.pd_port.local_snk_cap.number - 1, 
                MSG_DATA_SINK_CAPABILITIES, tpc_port.pd_port.local_snk_cap.pdos);
        }
        break;
    case PD_PE_GIVE_SOURCE_CAP:
        pd_protocol_process_tx(FRAME_SOP, false, tpc_port.pd_port.local_src_cap.number, 
                               MSG_DATA_SOURCE_CAPABILITIES, tpc_port.pd_port.local_src_cap.pdos);
        break;
    case PD_PE_GIVE_SINK_CAP_EXT:
        pd_pe_give_sink_cap_ext_entry();
        break;
    case PD_PE_GIVE_SOURCE_CAP_EXT:
        pd_pe_give_source_cap_ext_entry();
        break;
    case PD_PE_GIVE_REVISION:
        data_object = PD_MSG_RMDO(3, 2, 1, 1);
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_REVISION, &data_object);
        break;
    case PD_PE_GET_SINK_CAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_GET_SINK_CAP, NULL);
        break;
    case PD_PE_GET_SOURCE_CAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_GET_SOURCE_CAP, NULL);
        break;
    case PD_PE_CHUNK_RECEIVED:
        pd_timer_enable(PD_TIMER_CHUNKINGNOTSUPPORTED, PD_TIMER_T_CHUNKINGNOTSUPPORTED, false);
        break;
    case PD_PE_SEND_NOT_SUPPORTED:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_NOT_SUPPORTED, NULL);
        break;
    default:
        break;
    }
}

static void pd_pe_prs_state_entry(uint8_t new_state)
{
    switch (new_state) {
    case PD_PE_PRS_SRC_SNK_EVALUATE_SWAP:
    case PD_PE_PRS_SNK_SRC_EVALUATE_SWAP:
        // always accept pr_swap
        tpc_port.pd_event = PD_EVENT_DPM_ACCEPT;
        break;
    case PD_PE_PRS_SRC_SNK_REJECT_SWAP:
    case PD_PE_PRS_SNK_SRC_REJECT_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_REJECT, NULL);
        break;
    case PD_PE_PRS_SRC_SNK_ACCEPT_SWAP:
    case PD_PE_PRS_SNK_SRC_ACCEPT_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_PRS_SRC_SNK_SEND_SWAP:
    case PD_PE_PRS_SNK_SRC_SEND_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_PR_SWAP, NULL);
        break;
    case PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF:
        tpc_port.pd_pe.in_pr_swap = true;
        tpc_port.pd_pe.explicit_contract = false;
        pd_timer_enable(PD_TIMER_SRCTRANSITION, PD_TIMER_T_SRCTRANSITION, false);
        break;
    case PD_PE_PRS_SNK_SRC_TRANSITION_TO_OFF:
        tpc_port.pd_pe.in_pr_swap = true;
        tpc_port.pd_pe.explicit_contract = false;
        pd_timer_enable(PD_TIMER_PSSOURCEOFF, PD_TIMER_T_PSSOURCEOFF, false);
        typec_ibus_control(0);
        break;
    case PD_PE_PRS_SRC_SNK_ASSERT_RD:
        tcpc_set_cc(TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD);
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    case PD_PE_PRS_SNK_SRC_ASSERT_RP:
        tcpc_set_cc(TCPC_ROLE_CTRL_CC_RP, TCPC_ROLE_CTRL_CC_RP);
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    case PD_PE_PRS_SRC_SNK_WAIT_SOURCE_ON:
        pd_timer_enable(PD_TIMER_PSSOURCEON, PD_TIMER_T_PSSOURCEON, false);
        tpc_port.pd_port.power_role = PD_POWER_ROLE_SNK;
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_PS_RDY, NULL);
        break;
    case PD_PE_PRS_SNK_SRC_SOURCE_ON:
        typec_vbus_control(true, 5000, 3000);
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
            tpc_port.pd_event = PD_EVENT_VBUS_VSAFE5V;
        }
        break;
    default:
        break;
    }
}

static void pd_pe_drs_state_entry(uint8_t new_state)
{
    switch (new_state) {
    case PD_PE_DRS_DFP_UFP_EVALUATE_SWAP:
    case PD_PE_DRS_UFP_DFP_EVALUATE_SWAP:
        // always accept dr_swap
        tpc_port.pd_event = PD_EVENT_DPM_ACCEPT;
        break;
    case PD_PE_DRS_DFP_UFP_REJECT_SWAP:
    case PD_PE_DRS_UFP_DFP_REJECT_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_REJECT, NULL);
        break;
    case PD_PE_DRS_DFP_UFP_ACCEPT_SWAP:
    case PD_PE_DRS_UFP_DFP_ACCEPT_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP:
    case PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP:
        /**
         * According to Lecroy TEST.PD.PROT.SRC.7: 
         * The Data_Role field of Accept Message shall not be updated.
         */
        if (new_state == PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP) {
            tpc_port.pd_port.data_role = PD_DATA_ROLE_UFP;
        } else {
            tpc_port.pd_port.data_role = PD_DATA_ROLE_DFP;
        }
        tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    case PD_PE_DRS_DFP_UFP_SEND_SWAP:
    case PD_PE_DRS_UFP_DFP_SEND_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_DR_SWAP, NULL);
        break;
    default:
        break;
    }
}

static void pd_pe_vcs_state_entry(uint8_t new_state)
{
    switch (new_state) {
    case PD_PE_VCS_EVALUATE_SWAP:
        tpc_port.pd_event = PD_EVENT_DPM_ACCEPT;
        break;
    case PD_PE_VCS_REJECT_VCONN_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_REJECT, NULL);
        break;
    case PD_PE_VCS_ACCEPT_SWAP:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_ACCEPT, NULL);
        break;
    case PD_PE_VCS_TURN_ON_VCONN:
        tcpc_set_vconn(true);
        pd_timer_enable(PD_TIMER_VCONNSOURCEON, PD_TIMER_T_VCONNSOURCEON, false);
        break;
    case PD_PE_VCS_SEND_PS_RDY:
        pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_PS_RDY, NULL);
        break;
    case PD_PE_VCS_WAIT_FOR_VCONN:
        pd_timer_enable(PD_TIMER_VCONNON, PD_TIMER_T_VCONNON, false);
        break;
    case PD_PE_VCS_TURN_OFF_VCONN:
        tcpc_set_vconn(false);
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    default:
        break;
    }
}

static void pd_pe_vdm_state_entry(uint8_t new_state)
{
    switch (new_state) {
    case PD_PE_RESP_VDM_GET_IDENTITY:
        pd_dpm_sync_svdm_revision();
        tpc_port.pd_event = PD_EVENT_DPM_ACCEPT;
        break;
    case PD_PE_RESP_VDM_GET_IDENTITY_NAK:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(tpc_port.pd_port.vdm_svid,
                                            VDM_VERSION_MAJOR_2x,
                                            tpc_port.pd_port.pd_vdm_revision[0],
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_ID);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(tpc_port.pd_port.vdm_svid,
                                            VDM_VERSION_MAJOR_10,
                                            VDM_VERSION_MINOR_20,
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_ID);
        }
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        break;
    case PD_PE_RESP_VDM_SEND_IDENTITY:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_2x,
                                                              tpc_port.pd_port.pd_vdm_revision[0],
                                                              0, MSG_DATA_VDM_CMD_TYPE_ACK,
                                                              MSG_DATA_VDM_CMD_DISCOVER_ID);
            tpc_port.pd_port.vdos[1] = PD_MSG_VDO_IDH(VDM_IDH_NO_USB_HOST, VDM_IDH_NO_USB_DEVICE, 
                                            VDM_IDH_UFPTYPE_PSD, VDM_IDH_MODAL_OP_NOT_SUPPORT, 
                                            VDM_IDH_DFPTYPE_USBHOST, VDM_IDH_CONNTYPE_RECEPTACLE, 
                                            VDM_IDH_USB_VENDOR_ID);
            pd_protocol_process_tx(FRAME_SOP, false, 5, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_10,
                                                              VDM_VERSION_MINOR_20,
                                                              0, MSG_DATA_VDM_CMD_TYPE_ACK,
                                                              MSG_DATA_VDM_CMD_DISCOVER_ID);
            tpc_port.pd_port.vdos[1] = PD_MSG_VDO_IDH20(VDM_IDH_NO_USB_HOST, VDM_IDH_NO_USB_DEVICE, \
                                    VDM_IDH_UFPTYPE_NOT_UFP, VDM_IDH_MODAL_OP_NOT_SUPPORT, VDM_IDH_USB_VENDOR_ID);
            pd_protocol_process_tx(FRAME_SOP, false, 4, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        }
        break;
    case PD_PE_RESP_VDM_GET_SVIDS_NAK:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_2x,
                                            tpc_port.pd_port.pd_vdm_revision[0],
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_SVIDS);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_10,
                                            VDM_VERSION_MINOR_20,
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_SVIDS);
        }
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        break;
    case PD_PE_RESP_VDM_GET_MODES_NAK:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_2x,
                                            tpc_port.pd_port.pd_vdm_revision[0],
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_MODES);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_10,
                                            VDM_VERSION_MINOR_20,
                                            0, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_DISCOVER_MODES);
        }
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        break;
    case PD_PE_RESP_VDM_ENTER_MODE_NAK:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_2x,
                                            tpc_port.pd_port.pd_vdm_revision[0],
                                            tpc_port.pd_port.vdm_mode_idx, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_ENTER_MODE);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_10,
                                            VDM_VERSION_MINOR_20,
                                            tpc_port.pd_port.vdm_mode_idx, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_ENTER_MODE);
        }
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        break;
    case PD_PE_RESP_VDM_EXIT_MODE_NAK:
        if (pd_is_rev30()) {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_2x,
                                            tpc_port.pd_port.pd_vdm_revision[0],
                                            tpc_port.pd_port.vdm_mode_idx, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_EXIT_MODE);
        } else {
            tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(0xFF00U, VDM_VERSION_MAJOR_10,
                                            VDM_VERSION_MINOR_20,
                                            tpc_port.pd_port.vdm_mode_idx, MSG_DATA_VDM_CMD_TYPE_NAK,
                                            MSG_DATA_VDM_CMD_EXIT_MODE);
        }
        pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        break;
    case PD_PE_INIT_PORT_VDM_IDENTITY_REQUEST:
        if ((tpc_port.pd_port.power_role == PD_POWER_ROLE_SNK) && \
            (!pd_protocol_check_sinktxok())) {
            tpc_port.pd_pe.pe_state = PD_PE_SNK_READY;
            pd_dpm_reactions_set(PD_DPM_REACTIONS_DISCOVER_ID);
            PD_PE_STATE_LOG("[%d]**PE_SNK_READY**\n", g_sys_ticks);
        } else {
            if (pd_is_rev30()) {
                tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(tpc_port.pd_port.vdm_svid,
                                                VDM_VERSION_MAJOR_2x,
                                                tpc_port.pd_port.pd_vdm_revision[0],
                                                0, MSG_DATA_VDM_CMD_TYPE_REQ,
                                                MSG_DATA_VDM_CMD_DISCOVER_ID);
            } else {
                tpc_port.pd_port.vdos[0] = PD_MSG_VDO_SVDM_HEADER(tpc_port.pd_port.vdm_svid,
                                                VDM_VERSION_MAJOR_10,
                                                VDM_VERSION_MINOR_20,
                                                0, MSG_DATA_VDM_CMD_TYPE_REQ,
                                                MSG_DATA_VDM_CMD_DISCOVER_ID);
            }
            pd_protocol_process_tx(FRAME_SOP, false, 1, MSG_DATA_VENDOR_DEFINED, tpc_port.pd_port.vdos);
        }
        break;
    case PD_PE_INIT_PORT_VDM_IDENTITY_ACKED:
    case PD_PE_INIT_PORT_VDM_IDENTITY_NAKED:
        if (new_state == PD_PE_INIT_PORT_VDM_IDENTITY_ACKED) {
            // evaluate discover id response
        }
        tpc_port.pd_event = PD_EVENT_DPM_READY;
        break;
    default:
        break;
    }
}

static bool timer_is_permanent(uint8_t timer)
{
    if ((timer == PD_TIMER_NORESPONSE) || \
        (timer == PD_TIMER_SINKPPSPERIODIC)) {
        return true;
    } else {
        return false;
    }
}

/* PD Policy Engine */
void pd_pe_state_transition_to(uint8_t new_state)
{
    bool __attribute__((unused)) later_print = true;

    /* Clear all events in the previous pe state */
    if (tpc_port.pd_event != PD_EVENT_CC_DETACH) {
        tpc_port.pd_event = PD_EVENT_NONE;
    }
    pd_timer_disable_all_not_permanent();
    if (!timer_is_permanent(tpc_port.pd_expired_timer))
        tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;

    tpc_port.pd_pe.pe_state_old = tpc_port.pd_pe.pe_state;
    tpc_port.pd_pe.pe_state = new_state;
#ifdef PD_PE_STATE_MACHINE_DBG
    if (!later_print) {
        PD_PE_STATE_LOG("[%d]**%s**\n", g_sys_ticks, pd_pe_state_string[tpc_port.pd_pe.pe_state]);
    }
#endif

    pd_pe_src_snk_state_exit(tpc_port.pd_pe.pe_state_old);

    // ERROR RECOVERY
    if (new_state == PD_PE_ERROR_RECOVERY) {
        pd_port_reset();
        PD_PE_STATE_LOG("[%d]**PE_ERROR_RECOVERY**\n", g_sys_ticks);
        typec_state_transition_to(TYPEC_ERRORRECOVERY);
        return;
    }
    // PHY BIST TEST MODE
    if (new_state == PD_PE_BIST_TEST_MODE) {
        tcpc_set_bist_test_mode(true);
        PD_PE_STATE_LOG("[%d]**PE_BIST_TEST_MODE**\n", g_sys_ticks);
        return;
    }
    // PHY BIST CARRIER MODE
    if (new_state == PD_PE_BIST_CARRIER_MODE) {
        tcpc_transmit(FRAME_BIST_CARRIER2, 0, 0, NULL);
        pd_timer_enable(PD_TIMER_BISTCONTMODE, PD_TIMER_T_BISTCONTMODE, false);
        PD_PE_STATE_LOG("[%d]**PE_BIST_CARRIER_MODE**\n", g_sys_ticks);
        return;
    }

    if ((new_state >= PD_PE_SRC_STARTUP) && (new_state <= PD_PE_SRC_READY)) {
        pd_pe_src_state_entry(new_state);
    } else if ((new_state >= PD_PE_SNK_STARTUP) && (new_state <= PD_PE_SNK_READY)) {
        pd_pe_snk_state_entry(new_state);
    } else if ((new_state >= PD_PE_GIVE_MANUFACTURER_INFO) && (new_state <= PD_PE_SEND_NOT_SUPPORTED)) {
        pd_pe_message_state_entry(new_state);
    } else if ((new_state >= PD_PE_PRS_SRC_SNK_EVALUATE_SWAP) && (new_state <= PD_PE_PRS_SNK_SRC_SOURCE_ON)) {
        pd_pe_prs_state_entry(new_state);
    } else if ((new_state >= PD_PE_DRS_DFP_UFP_EVALUATE_SWAP) && (new_state <= PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP)) {
        pd_pe_drs_state_entry(new_state);
    } else if ((new_state >= PD_PE_VCS_EVALUATE_SWAP) && (new_state <= PD_PE_VCS_TURN_OFF_VCONN)) {
        pd_pe_vcs_state_entry(new_state);
    } else if ((new_state >= PD_PE_RESP_VDM_GET_IDENTITY) && (new_state <= PD_PE_RESP_VDM_EXIT_MODE_NAK)) {
        pd_pe_vdm_state_entry(new_state);
    }

#ifdef PD_PE_STATE_MACHINE_DBG
    if (later_print)
        PD_PE_STATE_LOG("[%d]**%s**\n", g_sys_ticks, pd_pe_state_string[tpc_port.pd_pe.pe_state]);
#endif
}

void pd_process_vdm_msg(uint8_t *new_state)
{
    uint32_t vdm_hdr = tpc_port.pd_msg.payload[0];
//    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;
    uint8_t command_type, command;

    if (!PD_MSG_VDO_SVDM(vdm_hdr)) {
        *new_state = PD_PE_SEND_NOT_SUPPORTED;
        return;
    }

    if (PD_MSG_VDO_VID(vdm_hdr) != 0xFF00) {
        tpc_port.pd_port.vdm_svid = PD_MSG_VDO_VID(vdm_hdr);
        *new_state = PD_PE_RESP_VDM_GET_IDENTITY_NAK;
        return;
    } else {
        tpc_port.pd_port.vdm_svid = 0xFF00;
    }

    command_type = PD_MSG_VDO_CMD_TYPE(vdm_hdr);
    command = PD_MSG_VDO_CMD(vdm_hdr);
    if (command_type == MSG_DATA_VDM_CMD_TYPE_REQ) {
        switch (command) {
        case MSG_DATA_VDM_CMD_DISCOVER_ID:
            *new_state = PD_PE_RESP_VDM_GET_IDENTITY;
            break;
        case MSG_DATA_VDM_CMD_DISCOVER_SVIDS:
            *new_state = PD_PE_RESP_VDM_GET_SVIDS_NAK;
            break;
        case MSG_DATA_VDM_CMD_DISCOVER_MODES:
            *new_state = PD_PE_RESP_VDM_GET_MODES_NAK;
            break;
        case MSG_DATA_VDM_CMD_ENTER_MODE:
            tpc_port.pd_port.vdm_mode_idx = PD_MSG_VDO_OPJ_POS(vdm_hdr);
            *new_state = PD_PE_RESP_VDM_ENTER_MODE_NAK;
            break;
        case MSG_DATA_VDM_CMD_EXIT_MODE:
            tpc_port.pd_port.vdm_mode_idx = PD_MSG_VDO_OPJ_POS(vdm_hdr);
            *new_state = PD_PE_RESP_VDM_EXIT_MODE_NAK;
            break;
        case MSG_DATA_VDM_CMD_ATTENTION:
        default:
            break;
        }
    }
}

static void pd_pe_ready_handle_rx_msg()
{
    uint8_t type = PD_MSG_TYPE(tpc_port.pd_msg.msg_type);
    uint8_t subtype = PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type);
    uint8_t new_state = PD_PE_INVALID;
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;
    bool dfp = tpc_port.pd_port.data_role == PD_DATA_ROLE_DFP;
    uint16_t *ext_hdr = NULL;
    uint32_t bdo;

    if (type == MSG_TYPE_CONTROL) {
        switch (subtype) {
        case MSG_CTRL_ACCEPT:
        case MSG_CTRL_REJECT:
            new_state = src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET;
            break;
        case MSG_CTRL_GOODCRC:
        case MSG_CTRL_GOTOMIN:
        case MSG_CTRL_PING:
        case MSG_CTRL_PS_RDY:
        case MSG_CTRL_WAIT:
        case MSG_CTRL_NOT_SUPPORTED:
        case MSG_CTRL_DATA_RESET_COMPLETE:
            // ignore message
            break;
        case MSG_CTRL_DR_SWAP:
            new_state = dfp ? PD_PE_DRS_DFP_UFP_EVALUATE_SWAP : PD_PE_DRS_UFP_DFP_EVALUATE_SWAP;
            break;
        case MSG_CTRL_PR_SWAP:
            new_state = src ? PD_PE_PRS_SRC_SNK_EVALUATE_SWAP : PD_PE_PRS_SNK_SRC_EVALUATE_SWAP;
            break;
        case MSG_CTRL_VCONN_SWAP:
            new_state = PD_PE_VCS_EVALUATE_SWAP;
            break;
        case MSG_CTRL_GET_SOURCE_CAP:
            new_state = PD_PE_GIVE_SOURCE_CAP;
            break;
        case MSG_CTRL_GET_SINK_CAP:
            new_state = PD_PE_GIVE_SINK_CAP;
            break;
        case MSG_CTRL_GET_REVISION:
            new_state = PD_PE_GIVE_REVISION;
            break;
        case MSG_CTRL_GET_SOURCE_CAP_EXTENDED:
            new_state = PD_PE_GIVE_SOURCE_CAP_EXT;
            break;
        case MSG_CTRL_GET_SINK_CAP_EXTENDED:
            new_state = PD_PE_GIVE_SINK_CAP_EXT;
            break;
        case MSG_CTRL_DATA_RESET:
        case MSG_CTRL_GET_STATUS:
        case MSG_CTRL_FR_SWAP:
        case MSG_CTRL_GET_PPS_STATUS:
        case MSG_CTRL_GET_COUNTRY_CODES:
        case MSG_CTRL_GET_SOURCE_INFO:
        default:
            new_state = PD_PE_SEND_NOT_SUPPORTED;
            break;
        }
    } else if (type == MSG_TYPE_DATA) {
        switch (subtype) {
        case MSG_DATA_BIST:
            bdo = tpc_port.pd_msg.payload[0];
            if (((bdo >> 28) & 0xF) == 0x8) {
                // Rx BIST Test Mode
                pd_pe_state_transition_to(PD_PE_BIST_TEST_MODE);
            } else if (((bdo >> 28) & 0xF) == 0x5) {
                // Tx BIST Carreir Mode
                pd_pe_state_transition_to(PD_PE_BIST_CARRIER_MODE);
            }
            break;
        case MSG_DATA_SINK_CAPABILITIES:
        case MSG_DATA_BATTERY_STATUS:
        case MSG_DATA_SOURCE_INFO:
        case MSG_DATA_REVISION:
            // ignore message
            break;
        case MSG_DATA_SOURCE_CAPABILITIES:
            if (!src) {
                new_state = PD_PE_SNK_EVALUATE_CAPABILITY;
            }
            break;
        case MSG_DATA_REQUEST:
            if (src) {
                new_state = PD_PE_SRC_NEGOTIATE_CAPABILITY;
            }
            break;
        case MSG_DATA_VENDOR_DEFINED:
            pd_process_vdm_msg(&new_state);
            break;
        case MSG_DATA_ALERT:
        case MSG_DATA_GET_COUNTRY_INFO:
        case MSG_DATA_ENTER_USB:
        case MSG_DATA_EPR_REQUEST:
        case MSG_DATA_EPR_MODE:
        default:
            new_state = PD_PE_SEND_NOT_SUPPORTED;
            break;
        }
    } else if (type == MSG_TYPE_EXTENDED) {
        ext_hdr = (uint16_t *)tpc_port.pd_msg.payload;
        if (!PD_MSG_EXTHEADER_CHUNK(*ext_hdr)) {
            // not support unchunked ext message or chunked mechanism
            new_state = PD_PE_SEND_NOT_SUPPORTED;
        } else {
            if (PD_MSG_EXTHEADER_DATASIZE(*ext_hdr) > 26) {
                // multi-chunk
                new_state = PD_PE_CHUNK_RECEIVED;
            } else {
                switch (subtype) {
                case MSG_EXT_SOURCE_CAPABILITIES_EXTENDED:
                case MSG_EXT_STATUS:
                case MSG_EXT_BATTERY_CAPABILITIES:
                case MSG_EXT_MANUFACTURER_INFO:
                case MSG_EXT_SECURITY_RESPONSE:
                case MSG_EXT_FIRMWARE_UPDATE_RESPONSE:
                case MSG_EXT_PPS_STATUS:
                case MSG_EXT_COUNTRY_INFO:
                case MSG_EXT_COUNTRY_CODES:
                case MSG_EXT_SINK_CAPABILITIES_EXTENDED:
                    // ignore message
                    break;
                case MSG_EXT_GET_MANUFACTURER_INFO:
                    new_state = PD_PE_GIVE_MANUFACTURER_INFO;
                    break;
                case MSG_EXT_EXTENDED_CONTROL:
                case MSG_EXT_EPR_SOURCE_CAPABILITIES:
                case MSG_EXT_EPR_SINK_CAPABILITIES:
                case MSG_EXT_GET_BATTERY_CAP:
                case MSG_EXT_GET_BATTERY_STATUS:
                default:
                    new_state = PD_PE_SEND_NOT_SUPPORTED;
                    break;
                }
            }
        }
    }

    if (new_state) {
        pd_pe_state_transition_to(new_state);
    }
}

static void pd_pe_src_state_main(uint8_t state)
{
    switch (tpc_port.pd_pe.pe_state) {
    case PD_PE_SRC_STARTUP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_RST_DONE) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
        } else if (tpc_port.pd_expired_timer == PD_TIMER_SWAPSOURCESTART) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
        }
        break;
    case PD_PE_SRC_DISCOVERY:
        if (tpc_port.pd_expired_timer == PD_TIMER_SOURCECAPABILITY) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            if (tpc_port.pd_protocol.ncapscount <= 50) {
                pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
            } else {
                pd_pe_state_transition_to(PD_PE_SRC_DISABLED);
            }
        } else if ((tpc_port.pd_expired_timer == PD_TIMER_NORESPONSE) && \
                   (tpc_port.pd_protocol.nhardresetcount > 2)) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_DISABLED);
        }
        break;
    case PD_PE_SRC_SEND_CAPABILITIES:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.ncapscount = 0;
            tpc_port.pd_protocol.nhardresetcount = 0;
            pd_timer_disable(PD_TIMER_NORESPONSE);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if (tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_DISCOVERY);
            break;
        } else if (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_DATA) && \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_DATA_REQUEST)) {
                    pd_pe_state_transition_to(PD_PE_SRC_NEGOTIATE_CAPABILITY);
                } else {
                    pd_pe_state_transition_to(PD_PE_SRC_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
            } else if ((tpc_port.pd_expired_timer == PD_TIMER_NORESPONSE) && \
                    (tpc_port.pd_protocol.nhardresetcount > 2)) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                if (tpc_port.pd_port.pd_connected) {
                    pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
                } else {
                    pd_pe_state_transition_to(PD_PE_SRC_DISABLED);
                }
            }
        }
        break;
    case PD_PE_SRC_NEGOTIATE_CAPABILITY:
        if (tpc_port.pd_event == PD_EVENT_DPM_ACCEPT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_TRANSITION_SUPPLY1);
        } else if (tpc_port.pd_event == PD_EVENT_DPM_REJECT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_CAPABILITY_RESPONSE);
        }
        break;
    case PD_PE_SRC_TRANSITION_SUPPLY1:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SRCTRANSITION, PD_TIMER_T_SRCTRANSITION, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_expired_timer == PD_TIMER_SRCTRANSITION) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_SRC_TRANSITION_SUPPLY2);
            }
        }
        break;
    case PD_PE_SRC_TRANSITION_SUPPLY2:
        if (tpc_port.pd_expired_timer == PD_TIMER_SRCTRANSREQ) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_PS_RDY, NULL);
            if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
                tpc_port.pd_event = PD_EVENT_NONE;
                tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
                pd_pe_state_transition_to(PD_PE_SRC_READY);
            } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                       (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
                tpc_port.pd_event = PD_EVENT_NONE;
                tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
                pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
            }
        }
        break;
    case PD_PE_SRC_CAPABILITY_RESPONSE:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            if (tpc_port.pd_pe.explicit_contract) {
                pd_pe_state_transition_to(PD_PE_SRC_READY);
            } else {
                pd_pe_state_transition_to(PD_PE_SRC_WAIT_NEW_CAPABILITIES);
            }
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_SRC_WAIT_NEW_CAPABILITIES:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
        }
        break;
    case PD_PE_SRC_HARD_RESET:
    case PD_PE_SRC_HARD_RESET_RECEIVED:
        if (tpc_port.pd_expired_timer == PD_TIMER_PSHARDRESET) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            typec_vbus_control(false, 0, 0);
            pd_pe_state_transition_to(PD_PE_SRC_TRANSITION_TO_DEFAULT);
        }
        break;
    case PD_PE_SRC_TRANSITION_TO_DEFAULT:
        if (tpc_port.pd_expired_timer == PD_TIMER_SRCRECOVER) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            if (tpc_port.pd_port.need_vconn) {
                tcpc_set_vconn(true);
                typec_vbus_control(true, 5000, 3000);
                tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_SRC;
                if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
                    tpc_port.pd_event = PD_EVENT_VBUS_VSAFE5V;
                }
            }
        }
        if (tpc_port.pd_event == PD_EVENT_VBUS_VSAFE5V) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SRC_STARTUP);
        }
        break;
    case PD_PE_SRC_SEND_SOFT_RESET:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) || \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_ACCEPT)) {
                    pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
                } else {
                    pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
            }
        }
        break;
    case PD_PE_SRC_SOFT_RESET:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_SEND_CAPABILITIES);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET);
        }
        break;
    case PD_PE_SRC_READY:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            pd_pe_ready_handle_rx_msg();
        } else if (tpc_port.pd_expired_timer == PD_TIMER_DELAY_START_DPM) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_dpm_policy_start();
        }
        break;
    default:
        break;
    }
}

static void pd_pe_snk_state_main(uint8_t state)
{
    switch (tpc_port.pd_pe.pe_state) {
    case PD_PE_SNK_STARTUP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_RST_DONE) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_DISCOVERY);
        }
        break;
    case PD_PE_SNK_DISCOVERY:
        if (tpc_port.pd_event == PD_EVENT_VBUS_VSAFE5V) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_WAIT_FOR_CAPABILITIES);
        }
        break;
    case PD_PE_SNK_WAIT_FOR_CAPABILITIES:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_DATA) && \
                (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_DATA_SOURCE_CAPABILITIES)) {
                pd_pe_state_transition_to(PD_PE_SNK_EVALUATE_CAPABILITY);
            } else {
                pd_pe_state_transition_to(PD_PE_SNK_SEND_SOFT_RESET);
            }
        } else if (tpc_port.pd_expired_timer == PD_TIMER_SINKWAITCAP) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
        }
        break;
    case PD_PE_SNK_EVALUATE_CAPABILITY:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_SELECT_CAPABILITY);
        }
        break;
    case PD_PE_SNK_SELECT_CAPABILITY:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if (PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) {
                    if (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_ACCEPT) {
                        pd_pe_state_transition_to(PD_PE_SNK_TRANSITION_SINK);
                    } else if ((PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) || \
                                (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_REJECT)) {
                        if (tpc_port.pd_pe.explicit_contract) {
                            pd_pe_state_transition_to(PD_PE_SNK_READY);
                        } else {
                            pd_pe_state_transition_to(PD_PE_SNK_WAIT_FOR_CAPABILITIES);
                        }
                    } else {
                        pd_pe_state_transition_to(PD_PE_SNK_SEND_SOFT_RESET);
                    }
                } else {
                    pd_pe_state_transition_to(PD_PE_SNK_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
            }
        }
        break;
    case PD_PE_SNK_TRANSITION_SINK:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) && \
                (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_PS_RDY)) {
                pd_pe_state_transition_to(PD_PE_SNK_READY);
            } else {
                pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
            }
        } else if (tpc_port.pd_expired_timer == PD_TIMER_PSTRANSITION) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
        }
        break;
    case PD_PE_SNK_SEND_SOFT_RESET:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) || \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_ACCEPT)) {
                    pd_pe_state_transition_to(PD_PE_SNK_WAIT_FOR_CAPABILITIES);
                } else {
                    pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
            }
        }
        break;
    case PD_PE_SNK_SOFT_RESET:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SNK_WAIT_FOR_CAPABILITIES);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_SNK_HARD_RESET);
        }
        break;
    case PD_PE_SNK_TRANSITION_TO_DEFAULT:
        if (tpc_port.pd_event == PD_EVENT_VBUS_VSAFE0V) {
            pd_pe_state_transition_to(PD_PE_SNK_STARTUP);
        }
        break;
    case PD_PE_SNK_HARD_RESET:
        if (tpc_port.pd_event == PD_EVENT_HARD_RST_COMPLETE) {
            pd_pe_state_transition_to(PD_PE_SNK_TRANSITION_TO_DEFAULT);
        }
        break;
    case PD_PE_SNK_READY:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            pd_pe_ready_handle_rx_msg();
        } else if (tpc_port.pd_expired_timer == PD_TIMER_SINKPPSPERIODIC) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SNK_SELECT_CAPABILITY);
        } else if (tpc_port.pd_expired_timer == PD_TIMER_DELAY_START_DPM) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_dpm_policy_start();
        }
        break;
    default:
        break;
    }
}

static void pd_pe_message_state_main(uint8_t state)
{
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;
    uint8_t old_state = state;
    uint8_t i;

    switch (state) {
    case PD_PE_CHUNK_RECEIVED:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        } else if (tpc_port.pd_expired_timer == PD_TIMER_CHUNKINGNOTSUPPORTED) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_SEND_NOT_SUPPORTED);
        }
        break;
    case PD_PE_GIVE_MANUFACTURER_INFO:
    case PD_PE_GIVE_SINK_CAP:
    case PD_PE_GIVE_SOURCE_CAP:
    case PD_PE_GIVE_SINK_CAP_EXT:
    case PD_PE_GIVE_SOURCE_CAP_EXT:
    case PD_PE_GIVE_REVISION:
    case PD_PE_SEND_NOT_SUPPORTED:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
            if ((old_state == PD_PE_GIVE_SOURCE_CAP) && src) {
                tpc_port.pd_pe.pe_state_old = PD_PE_SRC_READY;
                tpc_port.pd_pe.pe_state = PD_PE_SRC_SEND_CAPABILITIES;
                pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            }
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_GET_SINK_CAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                    (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_DATA) && \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_DATA_SINK_CAPABILITIES)) {
                    tpc_port.pd_port.remote_snk_cap.number = PD_MSG_HEADER_CNT(tpc_port.pd_msg.msg_hdr);
                    for (i = 0; i < tpc_port.pd_port.remote_snk_cap.number; i++) {
                        tpc_port.pd_port.remote_snk_cap.pdos[i] = tpc_port.pd_msg.payload[i];
//                        PD_PE_STATE_LOG("remote snk cap pdo[%d]0x%08x\n", i, tpc_port.pd_msg.payload[i]);
                    }
                    pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
                } else {
                    tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                    pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            }
        }
        break;
    case PD_PE_GET_SOURCE_CAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                    (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_DATA) && \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_DATA_SOURCE_CAPABILITIES)) {
                    tpc_port.pd_port.remote_src_cap.number = PD_MSG_HEADER_CNT(tpc_port.pd_msg.msg_hdr);
                    for (i = 0; i < tpc_port.pd_port.remote_src_cap.number; i++) {
                        tpc_port.pd_port.remote_src_cap.pdos[i] = tpc_port.pd_msg.payload[i];
//                        PD_PE_STATE_LOG("remote src cap pdo[%d]0x%08x\n", i, tpc_port.pd_msg.payload[i]);
                    }
                    pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
                } else {
                    tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                    pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            }
        }
        break;
    default:
        break;
    }
}

static void pd_pe_prs_state_main(uint8_t state)
{
    uint8_t rp_lvl;
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;

    switch (state) {
    case PD_PE_PRS_SRC_SNK_EVALUATE_SWAP:
    case PD_PE_PRS_SNK_SRC_EVALUATE_SWAP:
        if (tpc_port.pd_event == PD_EVENT_DPM_ACCEPT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(src ? PD_PE_PRS_SRC_SNK_ACCEPT_SWAP : PD_PE_PRS_SNK_SRC_ACCEPT_SWAP);
        } else if (tpc_port.pd_event == PD_EVENT_DPM_REJECT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(src ? PD_PE_PRS_SRC_SNK_REJECT_SWAP : PD_PE_PRS_SNK_SRC_REJECT_SWAP);
        }
        break;
    case PD_PE_PRS_SRC_SNK_REJECT_SWAP:
    case PD_PE_PRS_SNK_SRC_REJECT_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_PRS_SRC_SNK_ACCEPT_SWAP:
    case PD_PE_PRS_SNK_SRC_ACCEPT_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF : PD_PE_PRS_SNK_SRC_TRANSITION_TO_OFF);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_PRS_SRC_SNK_SEND_SWAP:
    case PD_PE_PRS_SNK_SRC_SEND_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if (PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) {
                    if (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_ACCEPT) {
                        pd_pe_state_transition_to(src ? 
                                PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF : PD_PE_PRS_SNK_SRC_TRANSITION_TO_OFF);
                    } else if (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_REJECT) {
                        pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
                    } else {
                        pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
                    }
                } else {
                    pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            }
        }
        break;
    case PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF:
        if (tpc_port.pd_expired_timer == PD_TIMER_SRCTRANSITION) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            typec_vbus_control(false, 0, 0);
            if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
                tpc_port.pd_event = PD_EVENT_VBUS_VSAFE0V;
            }
        }
        if (tpc_port.pd_event == PD_EVENT_VBUS_VSAFE0V) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_PRS_SRC_SNK_ASSERT_RD);
        }
        break;
    case PD_PE_PRS_SNK_SRC_TRANSITION_TO_OFF:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) && \
                (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_PS_RDY)) {
                pd_pe_state_transition_to(PD_PE_PRS_SNK_SRC_ASSERT_RP);
            } else {
                pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
            }
        } else if (tpc_port.pd_expired_timer == PD_TIMER_PSSOURCEOFF) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
        }
        break;
    case PD_PE_PRS_SRC_SNK_ASSERT_RD:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_PRS_SRC_SNK_WAIT_SOURCE_ON);
        }
        break;
    case PD_PE_PRS_SNK_SRC_ASSERT_RP:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_PRS_SNK_SRC_SOURCE_ON);
        }
        break;
    case PD_PE_PRS_SRC_SNK_WAIT_SOURCE_ON:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) && \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_PS_RDY)) {
//                    if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
                        tpc_port.pd_pe.in_pr_swap = false;
                        tcpc_get_remote_rplvl(&rp_lvl, tpc_port.cc_orient);
                        if (rp_lvl == TYPEC_REMOTE_RP_3A) {
                            typec_ibus_control(3000);
                        } else if (rp_lvl == TYPEC_REMOTE_RP_1A5) {
                            typec_ibus_control(1500);
                        } else {
                            typec_ibus_control(500);
                        }
                        tpc_port.typec_state = TYPEC_ATTACHED_SNK;
                        pd_pe_state_transition_to(PD_PE_SNK_STARTUP);
//                    } else {
//                        pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
//                    }
                } else {
                    pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_PSSOURCEON) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
            }
        }
        break;
    case PD_PE_PRS_SNK_SRC_SOURCE_ON:
        if (tpc_port.pd_event == PD_EVENT_VBUS_VSAFE5V) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_port.power_role = PD_POWER_ROLE_SRC;
            tcpc_set_msg_header(tpc_port.pd_port.power_role, tpc_port.pd_port.data_role);
            pd_protocol_process_tx(FRAME_SOP, false, 0, MSG_CTRL_PS_RDY, NULL);
            if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
                tpc_port.pd_event = PD_EVENT_NONE;
                tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
                tpc_port.pd_pe.in_pr_swap = false;
                tpc_port.pd_pe.prs_to_src = true;
                tpc_port.typec_state = TYPEC_ATTACHED_SRC;
                pd_pe_state_transition_to(PD_PE_SRC_STARTUP);
            } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                       (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
                tpc_port.pd_event = PD_EVENT_NONE;
                tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
                pd_pe_state_transition_to(PD_PE_ERROR_RECOVERY);
            }
        }
        break;
    default:
        break;
    }
}

static void pd_pe_drs_state_main(uint8_t state)
{
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;
    bool dfp = tpc_port.pd_port.data_role == PD_DATA_ROLE_DFP;

    switch (state) {
    case PD_PE_DRS_DFP_UFP_EVALUATE_SWAP:
    case PD_PE_DRS_UFP_DFP_EVALUATE_SWAP:
        if (tpc_port.pd_event == PD_EVENT_DPM_ACCEPT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(dfp ? PD_PE_DRS_DFP_UFP_ACCEPT_SWAP : PD_PE_DRS_UFP_DFP_ACCEPT_SWAP);
        } else if (tpc_port.pd_event == PD_EVENT_DPM_REJECT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(dfp ? PD_PE_DRS_DFP_UFP_REJECT_SWAP : PD_PE_DRS_UFP_DFP_REJECT_SWAP);
        }
        break;
    case PD_PE_DRS_DFP_UFP_REJECT_SWAP:
    case PD_PE_DRS_UFP_DFP_REJECT_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_DRS_DFP_UFP_ACCEPT_SWAP:
    case PD_PE_DRS_UFP_DFP_ACCEPT_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(dfp ? PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP : PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP:
    case PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        }
        break;
    case PD_PE_DRS_DFP_UFP_SEND_SWAP:
    case PD_PE_DRS_UFP_DFP_SEND_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_SENDER_RESPONSE, PD_TIMER_T_SENDER_RESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                if (PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) {
                    if (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_ACCEPT) {
                        pd_pe_state_transition_to(dfp ? 
                            PD_PE_DRS_DFP_UFP_CHANGE_TO_UFP : PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP);
                    } else if (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_REJECT) {
                        pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
                    }
                } else {
                    pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_SENDER_RESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            }
        }
        break;
    default:
        break;
    }
}

static void pd_pe_vcs_state_main(uint8_t state)
{
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;

    switch (state) {
    case PD_PE_VCS_EVALUATE_SWAP:
        if (tpc_port.pd_event == PD_EVENT_DPM_ACCEPT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_VCS_ACCEPT_SWAP);
        } else if (tpc_port.pd_event == PD_EVENT_DPM_REJECT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_VCS_REJECT_VCONN_SWAP);
        }
        break;
    case PD_PE_VCS_REJECT_VCONN_SWAP:
    case PD_PE_VCS_ACCEPT_SWAP:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            if (state == PD_PE_VCS_REJECT_VCONN_SWAP) {
                pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
            } else {
                if (tpc_port.pd_port.vconn_role == PD_VCONN_ROLE_SRC) {
                    pd_pe_state_transition_to(PD_PE_VCS_WAIT_FOR_VCONN);
                } else {
                    pd_pe_state_transition_to(PD_PE_VCS_TURN_ON_VCONN);
                }
            }
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_VCS_TURN_ON_VCONN:
        if (tpc_port.pd_expired_timer == PD_TIMER_VCONNSOURCEON) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(PD_PE_VCS_SEND_PS_RDY);
        }
        break;
    case PD_PE_VCS_SEND_PS_RDY:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_SRC;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_VCS_WAIT_FOR_VCONN:
        if (tpc_port.pd_msg_received) {
            tpc_port.pd_msg_received = false;
            if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_CONTROL) && \
                (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_CTRL_PS_RDY)) {
                pd_pe_state_transition_to(PD_PE_VCS_TURN_OFF_VCONN);
            } else {
                pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            }
        } else if (tpc_port.pd_expired_timer == PD_TIMER_VCONNON) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to(src ? PD_PE_SRC_HARD_RESET : PD_PE_SNK_HARD_RESET);
        }
        break;
    case PD_PE_VCS_TURN_OFF_VCONN:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
            if (tpc_port.pd_port.vconn_role == PD_VCONN_ROLE_SRC) {
                tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_NONE;
            }
        }
        break;
    default:
        break;
    }
}

static void pd_pe_vdm_state_main(uint8_t state)
{
    bool src = tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC;

    switch (state) {
    case PD_PE_RESP_VDM_GET_IDENTITY:
        if (tpc_port.pd_event == PD_EVENT_DPM_ACCEPT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_RESP_VDM_SEND_IDENTITY);
        } else if (tpc_port.pd_event == PD_EVENT_DPM_REJECT) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_pe_state_transition_to(PD_PE_RESP_VDM_GET_IDENTITY_NAK);
        }
        break;
    case PD_PE_RESP_VDM_GET_IDENTITY_NAK:
    case PD_PE_RESP_VDM_GET_SVIDS_NAK:
    case PD_PE_RESP_VDM_GET_MODES_NAK:
    case PD_PE_RESP_VDM_ENTER_MODE_NAK:
    case PD_PE_RESP_VDM_EXIT_MODE_NAK:
    case PD_PE_RESP_VDM_SEND_IDENTITY:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                   (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
        }
        break;
    case PD_PE_INIT_PORT_VDM_IDENTITY_REQUEST:
        if (tpc_port.pd_event == PD_EVENT_PROTO_TX_SUCC) {
            tpc_port.pd_event = PD_EVENT_NONE;
            pd_timer_enable(PD_TIMER_VDMRESPONSE, PD_TIMER_T_VDMRESPONSE, false);
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
        } else if ((tpc_port.pd_event == PD_EVENT_PROTO_TX_FAILED) || \
                    (tpc_port.pd_event == PD_EVENT_PROTO_TX_DISCARD)) {
            tpc_port.pd_event = PD_EVENT_NONE;
            tpc_port.pd_protocol.prl_tx_state = PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
            pd_pe_state_transition_to(src ? PD_PE_SRC_SEND_SOFT_RESET : PD_PE_SNK_SEND_SOFT_RESET);
            break;
        }

        if (tpc_port.pd_protocol.prl_tx_state == PD_PRL_TX_WAIT_FOR_MESSAGE_REQUEST) {
            if (tpc_port.pd_msg_received) {
                tpc_port.pd_msg_received = false;
                // check vdm discover id response
                if ((PD_MSG_TYPE(tpc_port.pd_msg.msg_type) == MSG_TYPE_DATA) && \
                    (PD_MSG_SUB_TYPE(tpc_port.pd_msg.msg_type) == MSG_DATA_VENDOR_DEFINED) && \
                    (PD_MSG_VDO_VID(tpc_port.pd_msg.payload[0]) == 0xFF00)) {
                    if (PD_MSG_VDO_CMD_TYPE(tpc_port.pd_msg.payload[0]) == MSG_DATA_VDM_CMD_TYPE_ACK) {
                        pd_pe_state_transition_to(PD_PE_INIT_PORT_VDM_IDENTITY_ACKED);
                    } else if ((PD_MSG_VDO_CMD_TYPE(tpc_port.pd_msg.payload[0]) == MSG_DATA_VDM_CMD_TYPE_NAK) || \
                               (PD_MSG_VDO_CMD_TYPE(tpc_port.pd_msg.payload[0]) == MSG_DATA_VDM_CMD_TYPE_BUSY)) {
                        pd_pe_state_transition_to(PD_PE_INIT_PORT_VDM_IDENTITY_NAKED);
                    }
                }
            } else if (tpc_port.pd_expired_timer == PD_TIMER_VDMRESPONSE) {
                tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
                pd_pe_state_transition_to(PD_PE_INIT_PORT_VDM_IDENTITY_NAKED);
            }
        }
        break;
    case PD_PE_INIT_PORT_VDM_IDENTITY_ACKED:
    case PD_PE_INIT_PORT_VDM_IDENTITY_NAKED:
        if (tpc_port.pd_event == PD_EVENT_DPM_READY) {
            pd_pe_state_transition_to(src ? PD_PE_SRC_READY : PD_PE_SNK_READY);
        }
        break;
    default:
        break;
    }
}

static void pd_run_state_machine(void)
{
    uint8_t state;
    uint8_t type, sub_type;

    /* Delay start up SRC or SNK state machine */
    if (tpc_port.pd_expired_timer == PD_TIMER_PE_STARTUP) {
        tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
        if (tpc_port.typec_state == TYPEC_ATTACHED_SRC) {
            pd_pe_state_transition_to(PD_PE_SRC_STARTUP);
        } else if (tpc_port.typec_state == TYPEC_ATTACHED_SNK) {
            pd_pe_state_transition_to(PD_PE_SNK_STARTUP);
        }
        return;
    }

    /* Disconnect PD */
    if (tpc_port.pd_event == PD_EVENT_CC_DETACH) {
        tpc_port.pd_event = PD_EVENT_NONE;
        if (tpc_port.pd_pe.pe_state == PD_PE_BIST_TEST_MODE) {
            tcpc_set_bist_test_mode(false);
        }
        pd_port_reset();
        return;
    }

    if (tpc_port.pd_msg_received) {
        // Hard Reset
        if (tpc_port.pd_msg.frame_type == FRAME_HARD_RESET) {
            tpc_port.pd_msg_received = false;
            if (tpc_port.typec_state == TYPEC_ATTACHED_SRC) {
                pd_pe_state_transition_to(PD_PE_SRC_HARD_RESET_RECEIVED);
            } else if (tpc_port.typec_state == TYPEC_ATTACHED_SNK) {
                tpc_port.pd_pe.in_hard_reset = true;
                pd_pe_state_transition_to(PD_PE_SNK_TRANSITION_TO_DEFAULT);
            }
            return;
        }

        if (tpc_port.pd_msg.frame_type != FRAME_SOP) {
            // ignore SOPP and SOPPP messages
            tpc_port.pd_msg.msg_type = 0;
            tpc_port.pd_msg_received = false;
            return;
        }

        if (PD_MSG_HEADER_EXT(tpc_port.pd_msg.msg_hdr)) {
            type = MSG_TYPE_EXTENDED;
        } else {
            if (PD_MSG_HEADER_CNT(tpc_port.pd_msg.msg_hdr)) {
                type = MSG_TYPE_DATA;
            } else {
                type = MSG_TYPE_CONTROL;
            }
        }
        sub_type = PD_MSG_HEADER_TYPE(tpc_port.pd_msg.msg_hdr);
        tpc_port.pd_msg.msg_type = ((type & 0x7) << 5) | sub_type;
//        PD_PE_STATE_LOG("rx msg [%d].[%d]\n", type, sub_type);

        // Receive Soft_Reset shall be handled immediately
        if ((type == MSG_TYPE_CONTROL) && (sub_type == MSG_CTRL_SOFT_RESET)) {
            tpc_port.pd_msg_received = false;
            if (tpc_port.typec_state == TYPEC_ATTACHED_SRC) {
                pd_pe_state_transition_to(PD_PE_SRC_SOFT_RESET);
            } else if (tpc_port.typec_state == TYPEC_ATTACHED_SNK) {
                pd_pe_state_transition_to(PD_PE_SNK_SOFT_RESET);
            }
            return;
        }

        /**
         * [Patch] In the test case Lecroy TEST.PD.PROT.ALL3, DUT respond to
         * Get_Manufacturer_Info from Exerciser with GoodCRC, in which reserved
         * bit set to 1. So Exerciser will send another GoodCRC immediately, and
         * this will lead to DUT Tx discarded.
         */
        if ((type == MSG_TYPE_CONTROL) && (sub_type == MSG_CTRL_GOODCRC)) {
            tpc_port.pd_msg_received = false;
            return;
        }
    }

    state = tpc_port.pd_pe.pe_state;
    if (state == PD_PE_BIST_CARRIER_MODE) {
        if (tpc_port.pd_expired_timer == PD_TIMER_BISTCONTMODE) {
            tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
            pd_pe_state_transition_to((tpc_port.pd_port.power_role == PD_POWER_ROLE_SRC) ? \
                                    PD_PE_SRC_READY : PD_PE_SNK_READY);
        }
        return;
    }
    if ((state >= PD_PE_SRC_STARTUP) && (state <= PD_PE_SRC_READY)) {
        pd_pe_src_state_main(state);
    } else if ((state >= PD_PE_SNK_STARTUP) && (state <= PD_PE_SNK_READY)) {
        pd_pe_snk_state_main(state);
    } else if ((state >= PD_PE_GIVE_MANUFACTURER_INFO) && (state <= PD_PE_SEND_NOT_SUPPORTED)) {
        pd_pe_message_state_main(state);
    } else if ((state >= PD_PE_PRS_SRC_SNK_EVALUATE_SWAP) && (state <= PD_PE_PRS_SNK_SRC_SOURCE_ON)) {
        pd_pe_prs_state_main(state);
    } else if ((state >= PD_PE_DRS_DFP_UFP_EVALUATE_SWAP) && (state <= PD_PE_DRS_UFP_DFP_CHANGE_TO_DFP)) {
        pd_pe_drs_state_main(state);
    } else if ((state >= PD_PE_VCS_EVALUATE_SWAP) && (state <= PD_PE_VCS_TURN_OFF_VCONN)) {
        pd_pe_vcs_state_main(state);
    } else if ((state >= PD_PE_RESP_VDM_GET_IDENTITY) && (state <= PD_PE_RESP_VDM_EXIT_MODE_NAK)) {
        pd_pe_vdm_state_main(state);
    }
}

void msg_rx_fifo_init(void)
{
    tpc_port.pd_msg_rxf.r_idx = 0;
    tpc_port.pd_msg_rxf.w_idx = 0;
    tpc_port.pd_msg_rxf.cnt = 0;
//    memset(tpc_port.pd_msg_rxf.buf, 0, PD_MSG_RX_FIFO_SIZE * sizeof(struct pd_message));
}

static void msg_rx_fifo_put(struct pd_message msg)
{
    struct pd_msg_rx_fifo *fifo = &tpc_port.pd_msg_rxf;
    // disable IRQ if receive message in ISR

    if (fifo->cnt >= PD_MSG_RX_FIFO_SIZE) {
        printf("rx fifo overflow!\n");
        return;
    }

    memcpy(&fifo->buf[fifo->w_idx], &msg, sizeof(struct pd_message));
    fifo->w_idx = (fifo->w_idx + 1) & (PD_MSG_RX_FIFO_SIZE - 1);
    fifo->cnt++;
}

static void msg_rx_fifo_get(struct pd_message *msg)
{
    struct pd_msg_rx_fifo *fifo = &tpc_port.pd_msg_rxf;

    if (fifo->cnt == 0) {
        printf("rx fifo empty!\n");
        return;
    }

    memcpy(msg, &fifo->buf[fifo->r_idx], sizeof(struct pd_message));
    fifo->r_idx = (fifo->r_idx + 1) & (PD_MSG_RX_FIFO_SIZE - 1);
    fifo->cnt--;
}

static bool msg_rx_fifo_is_empty(void)
{
    return (tpc_port.pd_msg_rxf.cnt == 0);
}

static bool msg_rx_fifo_is_full(void)
{
    return (tpc_port.pd_msg_rxf.cnt >= PD_MSG_RX_FIFO_SIZE);
}

void pd_main_task(void)
{
    int ret;
    uint32_t alert_status;
    struct pd_message tmp_msg;

    while (typec_check_alert()) {
        ret = tcpc_get_alert_status(&alert_status);
        if (ret)
            return;
        
        if (!(alert_status & (TCPC_ALERT_RX_HARD_RST | TCPC_ALERT_RX_OVERFLOW | TCPC_ALERT_RX_STATUS))) {
            // no rx msg irq
            break;
        }

        if (alert_status & TCPC_ALERT_RX_HARD_RST) {
            // Hard Reset shall be handled at first
            tpc_port.pd_msg.frame_type = FRAME_HARD_RESET;
            tpc_port.pd_msg_received = true;
            tcpc_alert_status_clear(TCPC_ALERT_RX_HARD_RST | TCPC_ALERT_RX_OVERFLOW | TCPC_ALERT_RX_STATUS);
            // Hard Reset shall flush all previous rx messages
            msg_rx_fifo_init();
            break;
        } 

        if (alert_status & (TCPC_ALERT_RX_OVERFLOW | TCPC_ALERT_RX_STATUS)) {
            tcpc_get_rx_message(&tmp_msg.frame_type, &tmp_msg.msg_hdr, tmp_msg.payload);

            // clear rx_status flag will flush rx buffer, so it shall be done after get_rx_message
            if (alert_status & TCPC_ALERT_RX_OVERFLOW) {
                tcpc_alert_status_clear(TCPC_ALERT_RX_OVERFLOW | TCPC_ALERT_RX_STATUS);
            } else {
                tcpc_alert_status_clear(TCPC_ALERT_RX_STATUS);
            }

            if (!msg_rx_fifo_is_full()) {
                msg_rx_fifo_put(tmp_msg);
            }
        }
    }

    if ((!tpc_port.pd_msg_received) && (!msg_rx_fifo_is_empty())) {
        msg_rx_fifo_get(&tpc_port.pd_msg);
        tpc_port.pd_msg_received = true;
    }

    tpc_port.pd_expired_timer = pd_expired_timer();
    if (pd_is_rev30() && (tpc_port.pd_expired_timer == PD_TIMER_SINKTX)) {
        tpc_port.pd_expired_timer = TYPEC_PD_TIMER_NONE;
        tpc_port.dpm_tx_ready = true;
    }
    /* pd timer, receive message, transmit message, dpm trigger */
    if (tpc_port.pd_expired_timer || tpc_port.pd_event || tpc_port.pd_msg_received) {
        pd_run_state_machine();
    }
}

void pd_dpm_reactions_set(uint32_t flag)
{
    tpc_port.dpm_reactions |= flag;
    PD_PE_STATE_LOG("set dpm reactions 0x%x\n", flag);
}

void pd_dpm_reactions_clear(uint32_t flag)
{
    tpc_port.dpm_reactions &= ~flag;
    PD_PE_STATE_LOG("clear dpm reactions 0x%x\n", flag);
}

void pd_dpm_main_task(void)
{
    /**
     * TODO:
     * 1. Discover Identity
     * 2. Get remote snk cap as SRC and remote src/snk cap as SNK
     * 3. Send DR_Swap if prefer as DFP
     * 4. Send PR_Swap if perfer as SNK
     */
    if (tpc_port.dpm_ongoing) {
        /* Hard Reset or CC Detach */
        if (!tpc_port.pd_pe.explicit_contract) {
            tpc_port.dpm_ongoing = false;
            tpc_port.dpm_tx_ready = false;
            return;
        }
        if ((tpc_port.pd_pe.pe_state != PD_PE_SRC_READY) && (tpc_port.pd_pe.pe_state != PD_PE_SNK_READY)) {
            return;
        }
        if (!tpc_port.dpm_reactions) {
            if ((tpc_port.pd_pe.pe_state == PD_PE_SRC_READY) && tpc_port.dpm_tx_ready) {
                if (pd_is_rev30()) {
                    pd_protocol_set_sink_tx(PD_PROTO_SINK_TX_OK);
                }
                tpc_port.dpm_tx_ready = false;
            }
            return;
        }

        /* PD 3.0 Source shall wait for SinkTxTimer to initiate new AMS */
        if (tpc_port.pd_pe.pe_state == PD_PE_SRC_READY) {
            if (pd_is_rev30()) {
                if ((!tpc_port.dpm_tx_ready) && (tpc_port.rp_level != PD_PROTO_SINK_TX_NG)) {
                    pd_timer_enable(PD_TIMER_SINKTX, PD_TIMER_T_SINKTX, true);
                    pd_protocol_set_sink_tx(PD_PROTO_SINK_TX_NG);
                }
            } else {
                tpc_port.dpm_tx_ready = true;
            }
        }

        /**
         * Initiate new AMS if there are dpm_reactions to do
         * Send Discover ID at first
         */
        if ((tpc_port.pd_pe.pe_state == PD_PE_SRC_READY) && tpc_port.dpm_tx_ready) {
            pd_dpm_reactions_process();
        } else if ((tpc_port.pd_pe.pe_state == PD_PE_SNK_READY) && pd_protocol_check_sinktxok()) {
            pd_dpm_reactions_process();
        }
    }

    return;
}
